Sub: Introduced AUTO_YAW_RATE mode and its features.

ArduSub/defines.h

@@ -26,7 +26,8 @@ enum autopilot_yaw_mode {
     AUTO_YAW_LOOK_AT_HEADING =   3,  // point towards a particular angle (not pilot input accepted)
     AUTO_YAW_LOOK_AHEAD =        4,  // point in the direction the vehicle is moving
     AUTO_YAW_RESETTOARMEDYAW =   5,  // point towards heading at time motors were armed
-    AUTO_YAW_CORRECT_XTRACK =    6   // steer the sub in order to correct for crosstrack error during line following
+    AUTO_YAW_CORRECT_XTRACK =    6,  // steer the sub in order to correct for crosstrack error during line following
+    AUTO_YAW_RATE =              7   // steer the sub with the desired yaw rate 
 };
 
 // Acro Trainer types

ArduSub/mode.h

@@ -297,6 +297,7 @@ class ModeGuided : public Mode
     float get_auto_heading();
     void guided_limit_clear();
     void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode);
+    void set_yaw_rate(float turn_rate_dps);
 
 protected:
 

ArduSub/mode_auto.cpp

@@ -377,6 +377,17 @@ void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps
     // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction.  1 = clockwise, -1 = counterclockwise
 }
 
+
+// sets the desired yaw rate
+void ModeGuided::set_yaw_rate(float turn_rate_dps)
+{    
+    // set sub to desired yaw rate
+    sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE);    // deg / sec
+
+    // set yaw mode
+    set_auto_yaw_mode(AUTO_YAW_RATE);
+}
+
 // set_auto_yaw_roi - sets the yaw to look at roi for auto mode
 void ModeAuto::set_auto_yaw_roi(const Location &roi_location)
 {

ArduSub/mode_guided.cpp

@@ -577,6 +577,10 @@ void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)
     case AUTO_YAW_RESETTOARMEDYAW:
         // initial_armed_bearing will be set during arming so no init required
         break;
+
+    case AUTO_YAW_RATE:
+        // set target yaw rate to yaw_look_at_heading_slew
+        break;
     }
 }