From 6d4d1bc20a655eee62847466078941dd4709dacc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 May 2019 15:35:46 +0900 Subject: [PATCH] AR_AttitudeControl: add get_turn_rate_from_heading --- libraries/APM_Control/AR_AttitudeControl.cpp | 13 +++++++++++-- libraries/APM_Control/AR_AttitudeControl.h | 4 ++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index d6ee9aed58..6b1f4fa64b 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/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 diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index 70788d2761..af9c2fb314 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/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