diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1fed0b6746..521f50a482 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -634,8 +634,8 @@ static uint32_t throttle_integrator; //////////////////////////////////////////////////////////////////////////////// // auto flight mode's yaw mode static uint8_t auto_yaw_mode = AUTO_YAW_LOOK_AT_NEXT_WP; -// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION -static Vector3f yaw_look_at_WP; +// Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI +static Vector3f roi_WP; // bearing from current location to the yaw_look_at_WP static float yaw_look_at_WP_bearing; // yaw used for YAW_LOOK_AT_HEADING yaw_mode diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index f4fdb92074..96e9da7277 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -73,16 +73,16 @@ static float get_pilot_desired_yaw_rate(int16_t stick_angle) * yaw controllers *************************************************************/ -// get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller +// get_roi_yaw - returns heading towards location held in roi_WP // should be called at 100hz -static float get_look_at_yaw() +static float get_roi_yaw() { - static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz + static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 10hz - look_at_yaw_counter++; - if (look_at_yaw_counter >= 10) { - look_at_yaw_counter = 0; - yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP); + roi_yaw_counter++; + if (roi_yaw_counter >= 10) { + roi_yaw_counter = 0; + yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), roi_WP); } return yaw_look_at_WP_bearing; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index e98ecd7772..70bc5b46d4 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1365,8 +1365,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) param1 = tell_command.p1; break; - case MAV_CMD_NAV_ROI: - case MAV_CMD_DO_SET_ROI: + case MAV_CMD_NAV_ROI: // 80 + case MAV_CMD_DO_SET_ROI: // 201 param1 = tell_command.p1; // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters x = tell_command.lat/1.0e7f; // local (x), global (latitude) y = tell_command.lng/1.0e7f; // local (y), global (longitude) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 8c522a4840..e84311b60b 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -658,26 +658,33 @@ static void do_set_home() // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint static void do_roi() { + // if location is zero lat, lon and altitude turn off 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 + }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 ) { - yaw_look_at_WP = pv_location_to_vector(command_cond_queue); - set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION); - } - // send the command to the camera mount - camera_mount.set_roi_cmd(&command_cond_queue); - - // 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) + // 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(command_cond_queue); + set_auto_yaw_mode(AUTO_YAW_ROI); + } + // send the command to the camera mount + camera_mount.set_roi_cmd(&command_cond_queue); + + // 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 - yaw_look_at_WP = pv_location_to_vector(command_cond_queue); - set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION); + // 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 + } } // do_take_picture - take a picture with the camera library diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 80c89fa23d..40865be963 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -21,6 +21,11 @@ static bool auto_init(bool ignore_checks) { if ((GPS_ok() && inertial_nav.position_ok() && g.command_total > 1) || ignore_checks) { + // stop ROI from carrying over from previous runs of the mission + // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check + if (auto_yaw_mode == AUTO_YAW_ROI) { + set_auto_yaw_mode(AUTO_YAW_HOLD); + } // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function init_commands(); return true; @@ -117,7 +122,10 @@ static void auto_wp_start(const Vector3f& destination) wp_nav.set_wp_destination(destination); // initialise yaw - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI + if (auto_yaw_mode != AUTO_YAW_ROI) { + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + } } // auto_wp_run - runs the auto waypoint controller @@ -296,10 +304,10 @@ void set_auto_yaw_mode(uint8_t yaw_mode) switch (auto_yaw_mode) { case AUTO_YAW_LOOK_AT_NEXT_WP: - // original_wp_bearing will be set by do_nav_wp or other nav command initialisatoin functions so no init required + // original_wp_bearing will be set by do_nav_wp or other nav command initialisation functions so no init required break; - case AUTO_YAW_LOOK_AT_LOCATION: + case AUTO_YAW_ROI: // point towards a location held in yaw_look_at_WP yaw_look_at_WP_bearing = ahrs.yaw_sensor; break; @@ -326,9 +334,9 @@ float get_auto_heading(void) { switch(auto_yaw_mode) { - case AUTO_YAW_LOOK_AT_LOCATION: - // point towards a location held in yaw_look_at_WP - return get_look_at_yaw(); + case AUTO_YAW_ROI: + // point towards a location held in roi_WP + return get_roi_yaw(); break; case AUTO_YAW_LOOK_AT_HEADING: diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 39b23a21d1..43c0bb63d7 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -17,7 +17,7 @@ // ------------ #define AUTO_YAW_HOLD 0 // pilot controls the heading #define AUTO_YAW_LOOK_AT_NEXT_WP 1 // point towards next waypoint (no pilot input accepted) -#define AUTO_YAW_LOOK_AT_LOCATION 2 // point towards a location held in yaw_look_at_WP (no pilot input accepted) +#define AUTO_YAW_ROI 2 // point towards a location held in roi_WP (no pilot input accepted) #define AUTO_YAW_LOOK_AT_HEADING 3 // point towards a particular angle (not pilot input accepted) #define AUTO_YAW_LOOK_AHEAD 4 // point in the direction the copter is moving #define AUTO_YAW_RESETTOARMEDYAW 5 // point towards heading at time motors were armed