From e02375022ed26e36e174de083ae61979d4cc6be8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Jun 2018 13:16:16 +0900 Subject: [PATCH] AR_AttitudeControl: remove constraint on steering output --- libraries/APM_Control/AR_AttitudeControl.cpp | 2 +- libraries/APM_Control/AR_AttitudeControl.h | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index df2e7e0d81..c97c5022b1 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -285,7 +285,7 @@ float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_l const float d = _steer_rate_pid.get_d(); // constrain and return final output - return constrain_float(ff + p + i + d, -1.0f, 1.0f); + return (ff + p + i + d); } // get latest desired turn rate in rad/sec (recorded during calls to get_steering_out_rate) diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index bc7f41cd69..db6f48cfa9 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -43,15 +43,18 @@ public: // steering controller // - // return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s. + // return a steering servo output given a desired lateral acceleration rate in m/s/s. // positive lateral acceleration is to the right. dt should normally be the main loop rate. + // return value is normally in range -1.0 to +1.0 but can be higher or lower float get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt); - // return a steering servo output from -1 to +1 given a heading in radians + // return a steering servo output given a heading in radians + // 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, bool motor_limit_left, bool motor_limit_right, float dt); - // return a steering servo output from -1 to +1 given a - // desired yaw rate in radians/sec. Positive yaw is to the right. + // 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 float get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt); // get latest desired turn rate in rad/sec recorded during calls to get_steering_out_rate. For reporting purposes only