diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index eaa41364aa..f282c54c06 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4971,12 +4971,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc) } if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { - // switch off the camera tracking if enabled - if (mount->get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - mount->set_mode_to_default(); - } + mount->clear_roi_target(); } else { - // send the command to the camera mount mount->set_roi_target(roi_loc); } return MAV_RESULT_ACCEPTED;