diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8d1c2c9147..fa01a368e7 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1124,6 +1124,21 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_DO_SET_ROI: + // param1 : regional of interest mode (not supported) + // param2 : mission index/ target id (not supported) + // param3 : ROI index (not supported) + // param5 : x / lat + // param6 : y / lon + // param7 : z / alt + Location roi_loc; + roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); + roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); + roi_loc.alt = (int32_t)(packet.param7 * 100.0f); + set_auto_yaw_roi(roi_loc); + result = MAV_RESULT_ACCEPTED; + break; + case MAV_CMD_MISSION_START: if (set_mode(AUTO)) { result = MAV_RESULT_ACCEPTED; diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index abddd3c834..01e663df52 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -896,42 +896,10 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd) // do_roi - starts actions required by MAV_CMD_NAV_ROI // this involves either moving the camera to point at the ROI (region of interest) // and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature -// Note: the ROI should already be in the command_nav_queue global variable // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint static void do_roi(const AP_Mission::Mission_Command& cmd) { - // if location is zero lat, lon and altitude turn off ROI - if (auto_yaw_mode == AUTO_YAW_ROI && (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.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)); -#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 - if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) { - roi_WP = pv_location_to_vector(cmd.content.location); - set_auto_yaw_mode(AUTO_YAW_ROI); - } - // send the command to the camera mount - camera_mount.set_roi_cmd(&cmd.content.location); - - // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) - // 0: do nothing - // 1: point at next waypoint - // 2: point at a waypoint taken from WP# parameter (2nd parameter?) - // 3: point at a location given by alt, lon, lat parameters - // 4: point at a target given a target id (can't be implemented) -#else - // if we have no camera mount aim the quad at the location - roi_WP = pv_location_to_vector(cmd.content.location); - set_auto_yaw_mode(AUTO_YAW_ROI); -#endif // MOUNT == ENABLED - } + set_auto_yaw_roi(cmd.content.location); } // do_take_picture - take a picture with the camera library diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index d54a1e3da9..4b7d640ef5 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -485,6 +485,43 @@ static void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, u // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise } +// set_auto_yaw_roi - sets the yaw to look at roi for auto mode +static void set_auto_yaw_roi(const Location &roi_location) +{ + // if location is zero lat, lon and altitude turn off ROI + if (auto_yaw_mode == AUTO_YAW_ROI && (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.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)); +#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 + if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) { + roi_WP = pv_location_to_vector(roi_location); + set_auto_yaw_mode(AUTO_YAW_ROI); + } + // send the command to the camera mount + camera_mount.set_roi_cmd(&roi_location); + + // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) + // 0: do nothing + // 1: point at next waypoint + // 2: point at a waypoint taken from WP# parameter (2nd parameter?) + // 3: point at a location given by alt, lon, lat parameters + // 4: point at a target given a target id (can't be implemented) +#else + // if we have no camera mount aim the quad at the location + roi_WP = pv_location_to_vector(roi_location); + set_auto_yaw_mode(AUTO_YAW_ROI); +#endif // MOUNT == ENABLED + } +} + // get_auto_heading - returns target heading depending upon auto_yaw_mode // 100hz update rate float get_auto_heading(void)