From 6efc26d3a6bc5e1657198c5de4213bd35198aa0e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Mar 2014 16:02:14 +0900 Subject: [PATCH] Copter: restore mount's mode to default after ROI completes --- ArduCopter/commands_logic.pde | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index e84311b60b..dbdadf4a3d 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -662,7 +662,12 @@ static void do_roi() if (auto_yaw_mode == AUTO_YAW_ROI && (command_cond_queue.alt == 0 && command_cond_queue.lat == 0 && command_cond_queue.lng == 0)) { // set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command set_auto_yaw_mode(get_default_auto_yaw_mode(false)); - // To-Do: switch off the camera tracking if enabled +#if MOUNT == ENABLED + // switch off the camera tracking if enabled + if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + camera_mount.set_mode_to_default(); + } +#endif // MOUNT == ENABLED }else{ #if MOUNT == ENABLED // check if mount type requires us to rotate the quad @@ -683,7 +688,7 @@ static void do_roi() // if we have no camera mount aim the quad at the location roi_WP = pv_location_to_vector(command_cond_queue); set_auto_yaw_mode(AUTO_YAW_ROI); -#endif +#endif // MOUNT == ENABLED } }