Copter: DO_SET_ROI persists across waypoints

Users should add a DO_SET_ROI command with zero lat, lon and alt to
restore the yaw control to the default control (which is normally
look-at-next-wp)

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
+// Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI
-static Vector3f yaw_look_at_WP;
+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

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++;
+    roi_yaw_counter++;
-    if (look_at_yaw_counter >= 10) {
+    if (roi_yaw_counter >= 10) {
-        look_at_yaw_counter = 0;
+        roi_yaw_counter = 0;
-        yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
+        yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), roi_WP);
     }
 
     return yaw_look_at_WP_bearing;

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_NAV_ROI:       // 80
-        case MAV_CMD_DO_SET_ROI:
+        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)

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
+        // 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 ) {
+        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);
+            roi_WP = pv_location_to_vector(command_cond_queue);
-        set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION);
+            set_auto_yaw_mode(AUTO_YAW_ROI);
-    }
+        }
-    // send the command to the camera mount
+        // send the command to the camera mount
-    camera_mount.set_roi_cmd(&command_cond_queue);
+        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)
+        // 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
+        //		0: do nothing
-    //		1: point at next waypoint
+        //		1: point at next waypoint
-    //		2: point at a waypoint taken from WP# parameter (2nd parameter?)
+        //		2: point at a waypoint taken from WP# parameter (2nd parameter?)
-    //		3: point at a location given by alt, lon, lat parameters
+        //		3: point at a location given by alt, lon, lat parameters
-    //		4: point at a target given a target id (can't be implemented)
+        //		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
+        // if we have no camera mount aim the quad at the location
-    yaw_look_at_WP = pv_location_to_vector(command_cond_queue);
+        roi_WP = pv_location_to_vector(command_cond_queue);
-    set_auto_yaw_mode(AUTO_YAW_LOOK_AT_LOCATION);
+        set_auto_yaw_mode(AUTO_YAW_ROI);
 #endif
+    }
 }
 
 // do_take_picture - take a picture with the camera library

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:
+    case AUTO_YAW_ROI:
-        // point towards a location held in yaw_look_at_WP
+        // point towards a location held in roi_WP
-        return get_look_at_yaw();
+        return get_roi_yaw();
         break;
 
     case AUTO_YAW_LOOK_AT_HEADING:

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