APM_Control: G limit all turn rates

libraries/APM_Control/AR_AttitudeControl.cpp

@@ -394,6 +394,15 @@ const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
 
     AP_SUBGROUPINFO(_sailboat_heel_pid, "_SAIL_", 12, AR_AttitudeControl, AC_PID),
 
+    // @Param: _TURN_MAX_G
+    // @DisplayName: Turning maximum G force
+    // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
+    // @Units: gravities
+    // @Range: 0.1 10
+    // @Increment: 0.01
+    // @User: Standard
+    AP_GROUPINFO("_TURN_MAX_G", 13, AR_AttitudeControl, _turn_lateral_G_max, 0.6f),
+
     AP_GROUPEND
 };
 
@@ -485,6 +494,14 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l
         _desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad);
     }
 
+    // G limit based on speed
+    float speed;
+    if (get_forward_speed(speed)) {
+        // do not limit to less than 1 deg/s
+        const float turn_rate_max = MAX(get_turn_rate_from_lat_accel(get_turn_lat_accel_max(), speed), radians(1.0f));
+        _desired_turn_rate = constrain_float(_desired_turn_rate, -turn_rate_max, turn_rate_max);
+    }
+
     // set PID's dt
     _steer_rate_pid.set_dt(dt);
 

libraries/APM_Control/AR_AttitudeControl.h

@@ -86,6 +86,9 @@ public:
     // calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s)
     float get_turn_rate_from_lat_accel(float lat_accel, float speed) const;
 
+    // get the G limit lateral acceleration ( m/s/s), returning at least 0.1G
+    float get_turn_lat_accel_max() const { return MAX(_turn_lateral_G_max, 0.1f) * GRAVITY_MSS; }
+
     //
     // throttle / speed controller
     //
@@ -167,6 +170,7 @@ private:
     AP_Float _stop_speed;           // speed control stop speed.  Motor outputs to zero once vehicle speed falls below this value
     AP_Float _steer_accel_max;      // steering angle acceleration max in deg/s/s
     AP_Float _steer_rate_max;       // steering rate control maximum rate in deg/s
+    AP_Float _turn_lateral_G_max;   // sterring maximum lateral acceleration limit in 'G'
 
     // steering control
     uint32_t _steer_lat_accel_last_ms;  // system time of last call to lateral acceleration controller (i.e. get_steering_out_lat_accel)