From 873eb68d79376a46826a734c07fb188711ac4ce2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 4 Jul 2014 15:42:51 +0900 Subject: [PATCH] Plane: add support for DO_SET_ROI within mission --- ArduPlane/commands_logic.pde | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 64cb387236..a327a3b85f 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -143,13 +143,16 @@ start_command(const AP_Mission::Mission_Command& cmd) // system to control the vehicle attitude and the attitude of various // devices such as cameras. // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| - case MAV_CMD_NAV_ROI: - #if 0 - // send the command to the camera mount - camera_mount.set_roi_cmd(&cmd.content.location); - #else - gcs_send_text_P(SEVERITY_LOW, PSTR("DO_SET_ROI not supported")); - #endif + case MAV_CMD_DO_SET_ROI: + if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { + // switch off the camera tracking if enabled + if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + camera_mount.set_mode_to_default(); + } + } else { + // send the command to the camera mount + camera_mount.set_roi_cmd(&cmd.content.location); + } break; case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|