Rover: add pivot_turn_rate to g2

APMrover2/Parameters.cpp

@@ -555,6 +555,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
     AP_SUBGROUPINFO(avoid, "AVOID_", 19, ParametersG2, AC_Avoid),
 
+    // @Param: PIVOT_TURN_RATE
+    // @DisplayName: Pivot turn rate
+    // @Description: Desired pivot turn rate in deg/s.
+    // @Units: deg/s
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("PIVOT_TURN_RATE", 20, ParametersG2, pivot_turn_rate, 90),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -341,6 +341,9 @@ public:
 
     // avoidance library
     AC_Avoid avoid;
+
+    // pivot turn rate
+    AP_Int16 pivot_turn_rate;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/mode.cpp

@@ -363,7 +363,7 @@ void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct
 
     if (rover.use_pivot_steering(_yaw_error_cd)) {
         // for pivot turns use heading controller
-        calc_steering_to_heading(desired_heading);
+        calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
     } else {
         // call lateral acceleration to steering controller
         calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
@@ -392,10 +392,11 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse
 }
 
 // calculate steering output to drive towards desired heading
-void Mode::calc_steering_to_heading(float desired_heading_cd, bool reversed)
+void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max, bool reversed)
 {
     // calculate yaw error (in radians) and pass to steering angle controller
     const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f),
+                                                                         rate_max,
                                                                          g2.motors.limit.steer_left,
                                                                          g2.motors.limit.steer_right);
     g2.motors.set_steering(steering_out * 4500.0f);

APMrover2/mode.h

@@ -116,7 +116,7 @@ protected:
     void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false);
 
     // calculate steering output to drive towards desired heading
-    void calc_steering_to_heading(float desired_heading_cd, bool reversed = false);
+    void calc_steering_to_heading(float desired_heading_cd, float rate_max, bool reversed = false);
 
     // calculates the amount of throttle that should be output based
     // on things like proximity to corners and current speed