From 0d863aa736031112429b21a3bb603c877aede3c2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 14 May 2022 13:25:34 +0930 Subject: [PATCH] AC_AttitudeControl: Allow diabling of slew limit --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 11 ++++++++--- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 2 +- .../AC_AttitudeControl_Multi_6DoF.cpp | 2 +- .../AC_AttitudeControl_Multi_6DoF.h | 2 +- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 1e24000057..053a719e61 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -551,10 +551,15 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste } // Command a thrust vector and heading rate -void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds) +void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw) { // Convert from centidegrees on public interface to radians - const float heading_rate = radians(heading_rate_cds * 0.01f); + float heading_rate = radians(heading_rate_cds * 0.01f); + // a zero _angle_vel_yaw_max means that setting is disabled + const float slew_yaw_max_rads = get_slew_yaw_max_rads(); + if (slew_yaw) { + heading_rate = constrain_float(heading_rate, -slew_yaw_max_rads, slew_yaw_max_rads); + } // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z); @@ -580,7 +585,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust _ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt); // Limit the angular velocity - ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), get_slew_yaw_max_rads()); + ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); } else { Quaternion yaw_quat; yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate * _dt}); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index c9fba10612..4b9c6d63f1 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -184,7 +184,7 @@ public: virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd); // Command a thrust vector in the earth frame and a heading angle and/or rate - virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds); + virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true); virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds); void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp index 5cea0ab08b..6a2c75f2a8 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp @@ -49,7 +49,7 @@ void AC_AttitudeControl_Multi_6DoF::input_euler_angle_roll_pitch_yaw(float euler } // Command a thrust vector and heading rate -void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds) +void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw) { // convert thrust vector to a roll and pitch angles // this negates the advantage of using thrust vector control, but works just fine diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h index 76deec376f..d65784dd03 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h @@ -33,7 +33,7 @@ public: void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override; // Command a thrust vector in the earth frame and a heading angle and/or rate - void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds) override; + void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true) override; void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) override; /*