Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Copter, Sub, AP_Mount: Fix inconsistent handling of clearing ROIs #25277

Merged
merged 5 commits into from
Oct 29, 2023
Merged

Copter, Sub, AP_Mount: Fix inconsistent handling of clearing ROIs #25277

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b1dfd58
autotest: Add Copter test to verify clearing ROIs
nexton-winjeel Oct 16, 2023
9401fba
GCS_MAVLink: Make handler for DO_SET_ROI_NONE common
nexton-winjeel Oct 16, 2023
868fef3
AP_Mount: Remove handler for DO_SET_ROI_NONE
nexton-winjeel Oct 16, 2023
44ee5cd
Sub: Use AP_Mount::clear_roi_target() to remove ROI
nexton-winjeel Oct 16, 2023
490d2bb
Copter: Use AP_Mount::clear_roi_target() to remove ROI
nexton-winjeel Oct 16, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 1 addition & 3 deletions ArduCopter/autoyaw.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -154,9 +154,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
auto_yaw.set_mode_to_default(false);
#if HAL_MOUNT_ENABLED
// switch off the camera tracking if enabled
if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
rmackay9 marked this conversation as resolved.
Show resolved Hide resolved
copter.camera_mount.set_mode_to_default();
}
copter.camera_mount.clear_roi_target();
#endif // HAL_MOUNT_ENABLED
} else {
#if HAL_MOUNT_ENABLED
Expand Down
4 changes: 1 addition & 3 deletions ArduSub/mode_auto.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -394,9 +394,7 @@ void ModeAuto::set_auto_yaw_roi(const Location &roi_location)
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
#if HAL_MOUNT_ENABLED
// switch off the camera tracking if enabled
if (sub.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
sub.camera_mount.set_mode_to_default();
}
sub.camera_mount.clear_roi_target();
#endif // HAL_MOUNT_ENABLED
} else {
#if HAL_MOUNT_ENABLED
Expand Down
40 changes: 40 additions & 0 deletions Tools/autotest/arducopter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10005,6 +10005,7 @@ def tests1e(self):
self.MAV_CMD_NAV_LOITER_UNLIM,
self.MAV_CMD_NAV_RETURN_TO_LAUNCH,
self.MAV_CMD_NAV_VTOL_LAND,
self.clear_roi,
])
return ret

Expand Down Expand Up @@ -10310,6 +10311,45 @@ def MAV_CMD_NAV_VTOL_LAND(self):
command(mavutil.mavlink.MAV_CMD_NAV_LAND)
self.wait_mode('LAND')

def clear_roi(self):
'''ensure three commands that clear ROI are equivalent'''

self.upload_simple_relhome_mission([
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 20),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 20), # directly North, i.e. 0 degrees
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, 20), # directly North, i.e. 0 degrees
])

self.set_parameter("AUTO_OPTIONS", 3)
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
home_loc = self.mav.location()

cmd_ids = [
mavutil.mavlink.MAV_CMD_DO_SET_ROI,
mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,
mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE,
]
for command in self.run_cmd, self.run_cmd_int:
for cmd_id in cmd_ids:
self.wait_waypoint(2, 2)

# Set an ROI at the Home location, expect to point at Home
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, p5=home_loc.lat, p6=home_loc.lng, p7=home_loc.alt)
self.wait_heading(180)

# Clear the ROI, expect to point at the next Waypoint
self.progress("Clear ROI using %s(%d)" % (command.__name__, cmd_id))
command(cmd_id)
self.wait_heading(0)

self.wait_waypoint(4, 4)
self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=2)

self.land_and_disarm()

def start_flying_simple_rehome_mission(self, items):
'''uploads items, changes mode to auto, waits ready to arm and arms
vehicle. If the first item it a takeoff you can expect the
Expand Down
8 changes: 0 additions & 8 deletions libraries/AP_Mount/AP_Mount.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -505,12 +505,6 @@ MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t
return MAV_RESULT_ACCEPTED;
}

MAV_RESULT AP_Mount::handle_command_do_set_roi_none()
{
set_mode_to_default();
return MAV_RESULT_ACCEPTED;
}

MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
{
switch (packet.command) {
Expand All @@ -524,8 +518,6 @@ MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const m
return handle_command_do_gimbal_manager_configure(packet, msg);
case MAV_CMD_DO_SET_ROI_SYSID:
return handle_command_do_set_roi_sysid(packet);
case MAV_CMD_DO_SET_ROI_NONE:
return handle_command_do_set_roi_none();
default:
return MAV_RESULT_UNSUPPORTED;
}
Expand Down
3 changes: 0 additions & 3 deletions libraries/AP_Mount/AP_Mount.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,9 +181,6 @@ class AP_Mount
// handling of set_roi_sysid message
MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet);

// handling of set_roi_none message
MAV_RESULT handle_command_do_set_roi_none();

// mavlink message handling:
MAV_RESULT handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
void handle_param_value(const mavlink_message_t &msg);
Expand Down
7 changes: 6 additions & 1 deletion libraries/GCS_MAVLink/GCS_Common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5104,12 +5104,17 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
case MAV_CMD_DO_SET_MODE:
return handle_command_do_set_mode(packet);

case MAV_CMD_DO_SET_ROI_NONE: {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not really a fan of moving handling of this command up into the GCS layer. That seems like we're moving in the wrong direction.

I think the inconsistently comes from how Copter's special handling of ROI in that we want to be able to point the copter's nose at the ROI target in case the gimbal does not have yaw control.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Agreed, but moving it here is consistent with how DO_SET_ROI and DO_SET_ROI_LOCATION are currently handled.

I'd rather move this here now, then refactor all three of them together.

const Location zero_loc = Location();
return handle_command_do_set_roi(zero_loc);
}

case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_SET_ROI_LOCATION:
return handle_command_do_set_roi(packet);

#if HAL_MOUNT_ENABLED
case MAV_CMD_DO_SET_ROI_SYSID:
case MAV_CMD_DO_SET_ROI_NONE:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
Expand Down