From a9ee949c328376a80fdf01b15cc6134f2858ac83 Mon Sep 17 00:00:00 2001
From: Ammarf <35584084+Ammarf@users.noreply.github.com>
Date: Fri, 18 May 2018 16:04:17 +0900
Subject: [PATCH] AR_AttitudeControl: limit desired steering rate

---
 libraries/APM_Control/AR_AttitudeControl.cpp | 8 ++++++--
 libraries/APM_Control/AR_AttitudeControl.h   | 2 +-
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp
index 183af16ad9..2381822c80 100644
--- a/libraries/APM_Control/AR_AttitudeControl.cpp
+++ b/libraries/APM_Control/AR_AttitudeControl.cpp
@@ -203,13 +203,17 @@ float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool m
 }
 
 // return a steering servo output from -1 to +1 given a heading in radians
-float AR_AttitudeControl::get_steering_out_heading(float heading_rad, bool motor_limit_left, bool motor_limit_right)
+float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right)
 {
     // calculate heading error (in radians)
     const float yaw_error = wrap_PI(heading_rad - _ahrs.yaw);
 
     // Calculate the desired turn rate (in radians) from the angle error (also in radians)
-    const float desired_rate = _steer_angle_p.get_p(yaw_error);
+    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)) {
+        desired_rate = constrain_float(desired_rate, -rate_max, rate_max);
+    }
 
     return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right);
 }
diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h
index cea2b00b9f..335e6f55c0 100644
--- a/libraries/APM_Control/AR_AttitudeControl.h
+++ b/libraries/APM_Control/AR_AttitudeControl.h
@@ -48,7 +48,7 @@ public:
     float get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right);
 
     // return a steering servo output from -1 to +1 given a heading in radians
-    float get_steering_out_heading(float heading_rad, bool motor_limit_left, bool motor_limit_right);
+    float get_steering_out_heading(float heading_rad, float rate_max, bool motor_limit_left, bool motor_limit_right);
 
     // return a steering servo output from -1 to +1 given a
     // desired yaw rate in radians/sec. Positive yaw is to the right.