AR_AttitudeControl: add get_turn_rate_from_heading

libraries/APM_Control/AR_AttitudeControl.cpp

@@ -302,17 +302,26 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool m
 // return value is normally in range -1.0 to +1.0 but can be higher or lower
 float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt)
 {
-    // calculate heading error (in radians)
+    // calculate the desired turn rate (in radians) from the angle error (also in radians)
+    float desired_rate = get_turn_rate_from_heading(heading_rad, rate_max_rads);
+
+    return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
+}
+
+// return a desired turn-rate given a desired heading in radians
+float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const
+{
     const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw);
 
     // Calculate the desired turn rate (in radians) from the angle error (also in radians)
     float desired_rate = _steer_angle_p.get_p(yaw_error);
+
     // limit desired_rate if a custom pivot turn rate is selected, otherwise use ATC_STR_RAT_MAX
     if (is_positive(rate_max_rads)) {
         desired_rate = constrain_float(desired_rate, -rate_max_rads, rate_max_rads);
     }
 
-    return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
+    return desired_rate;
 }
 
 // return a steering servo output from -1 to +1 given a

libraries/APM_Control/AR_AttitudeControl.h

@@ -65,6 +65,10 @@ public:
     // return value is normally in range -1.0 to +1.0 but can be higher or lower
     float get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt);
 
+    // return a desired turn-rate given a desired heading in radians
+    // normally the results are later passed into get_steering_out_rate
+    float get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const;
+
     // return a steering servo output given a desired yaw rate in radians/sec.
     // positive yaw is to the right
     // return value is normally in range -1.0 to +1.0 but can be higher or lower