AR_AttitudeControl: add get_turn_rate_from_lat_accel

libraries/APM_Control/AR_AttitudeControl.cpp

@@ -292,17 +292,7 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool m
         return 0.0f;
     }
 
-    // enforce minimum speed to stop oscillations when first starting to move
-    if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
-        if (is_negative(speed)) {
-            speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
-        } else {
-            speed = AR_ATTCONTROL_STEER_SPEED_MIN;
-        }
-    }
-
-    // Calculate the desired steering rate given desired_accel and speed
-    const float desired_rate = desired_accel / speed;
+    const float desired_rate = get_turn_rate_from_lat_accel(desired_accel, speed);
 
     return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
 }
@@ -417,6 +407,21 @@ bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const
     return true;
 }
 
+// calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s)
+float AR_AttitudeControl::get_turn_rate_from_lat_accel(float lat_accel, float speed) const
+{
+    // enforce minimum speed to stop oscillations when first starting to move
+    if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
+        if (is_negative(speed)) {
+            speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
+        } else {
+            speed = AR_ATTCONTROL_STEER_SPEED_MIN;
+        }
+    }
+
+    return lat_accel / speed;
+}
+
 // return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards)
 //   motor_limit should be true if motors have hit their upper or lower limits
 //   cruise speed should be in m/s, cruise throttle should be a number from -1 to +1

libraries/APM_Control/AR_AttitudeControl.h

@@ -79,6 +79,9 @@ public:
     // get actual lateral acceleration in m/s/s.  returns true on success.  For reporting purposes only
     bool get_lat_accel(float &lat_accel) const;
 
+    // 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;
+
     //
     // throttle / speed controller
     //