AP_Mission: add support for Set-Yaw-Speed command

This is initially only intended for use by Rover

libraries/AP_Mission/AP_Mission.cpp

@@ -272,7 +272,8 @@ bool AP_Mission::replace_cmd(uint16_t index, Mission_Command& cmd)
 /// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
 bool AP_Mission::is_nav_cmd(const Mission_Command& cmd)
 {
-    return (cmd.id <= MAV_CMD_NAV_LAST);
+    // NAV commands all have ids below MAV_CMD_NAV_LAST except NAV_SET_YAW_SPEED
+    return (cmd.id <= MAV_CMD_NAV_LAST || cmd.id == MAV_CMD_NAV_SET_YAW_SPEED);
 }
 
 /// get_next_nav_cmd - gets next "navigation" command found at or after start_index
@@ -312,6 +313,14 @@ int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle)
     if (!get_next_nav_cmd(_nav_cmd.index+1, cmd)) {
         return default_angle;
     }
+    // special handling for nav commands with no target location
+    if (cmd.id == MAV_CMD_NAV_GUIDED_ENABLE ||
+        cmd.id == MAV_CMD_NAV_DELAY) {
+        return default_angle;
+    }
+    if (cmd.id == MAV_CMD_NAV_SET_YAW_SPEED) {
+        return (_nav_cmd.content.set_yaw_speed.angle_deg * 100);
+    }
     return get_bearing_cd(_nav_cmd.content.location, cmd.content.location);
 }
 
@@ -779,6 +788,12 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
         copy_location = true;
         break;
 
+    case MAV_CMD_NAV_SET_YAW_SPEED:
+        cmd.content.set_yaw_speed.angle_deg = packet.param1;        // target angle in degrees
+        cmd.content.set_yaw_speed.speed = packet.param2;            // speed in meters/second
+        cmd.content.set_yaw_speed.relative_angle = packet.param3;   // 0 = absolute angle, 1 = relative angle
+        break;
+
     default:
         // unrecognised command
         return MAV_MISSION_UNSUPPORTED;
@@ -1229,6 +1244,12 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
         packet.param1 = cmd.p1/100.0f; // copy max-descend parameter (m->cm)
         break;
 
+    case MAV_CMD_NAV_SET_YAW_SPEED:
+        packet.param1 = cmd.content.set_yaw_speed.angle_deg;        // target angle in degrees
+        packet.param2 = cmd.content.set_yaw_speed.speed;            // speed in meters/second
+        packet.param3 = cmd.content.set_yaw_speed.relative_angle;   // 0 = absolute angle, 1 = relative angle
+        break;
+
     default:
         // unrecognised command
         return false;

libraries/AP_Mission/AP_Mission.h

@@ -173,7 +173,14 @@ public:
         bool cold_start; // use cold start procedure
         uint16_t height_delay_cm; // height delay for start
     };
-    
+
+    // NAV_SET_YAW_SPEED support
+    struct PACKED Set_Yaw_Speed {
+        float angle_deg;        // target angle in degrees (0=north, 90=east)
+        float speed;            // speed in meters/second
+        uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle
+    };
+
     union PACKED Content {
         // jump structure
         Jump_Command jump;
@@ -228,13 +235,16 @@ public:
 
         // DO_ENGINE_CONTROL
         Do_Engine_Control do_engine_control;
-        
-        // location
-        Location location;      // Waypoint location
 
         // navigation delay
         Navigation_Delay_Command nav_delay;
 
+        // navigation delay
+        Set_Yaw_Speed set_yaw_speed;
+
+        // location
+        Location location;      // Waypoint location
+
         // raw bytes, for reading/writing to eeprom. Note that only 10 bytes are available
         // if a 16 bit command ID is used
         uint8_t bytes[12];