diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 3254dfde78946..40acda0f05c59 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -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) { - copter.camera_mount.set_mode_to_default(); - } + copter.camera_mount.clear_roi_target(); #endif // HAL_MOUNT_ENABLED } else { #if HAL_MOUNT_ENABLED diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index 87ad912976b5c..d947c8ed0e938 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -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 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 043c791540b57..b8c83d4fd921a 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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 @@ -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 diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 5f582ffdbbc00..c7f1701fbfe9e 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -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) { @@ -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; } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index d349da56b77b5..7e8dc661d6028 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -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); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3a8acdc913817..dfcc64c8681f8 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -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: { + 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: