From 5a1fdf9f7815d706ce9a55fb3da85169835c650b Mon Sep 17 00:00:00 2001 From: Peter Hall Date: Sun, 20 Jun 2021 17:56:43 +0100 Subject: [PATCH] AC_AttitudeControl: add thrust vector methods to 6DoF multi --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 4 ++-- .../AC_AttitudeControl/AC_AttitudeControl.h | 4 ++-- .../AC_AttitudeControl_Multi_6DoF.cpp | 20 +++++++++++++++++++ .../AC_AttitudeControl_Multi_6DoF.h | 4 ++++ 4 files changed, 28 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 9ffa07ab56..7dab5f407a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -538,7 +538,7 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste attitude_controller_run_quat(); } -// Command a Quaternion attitude with feedforward and smoothing +// Command a thrust vector and heading rate void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds) { // Convert from centidegrees on public interface to radians @@ -586,7 +586,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust attitude_controller_run_quat(); } -// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing +// Command a thrust vector, heading and heading rate void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) { // Convert from centidegrees on public interface to radians diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index c3ba38a443..49f7002d02 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -167,8 +167,8 @@ 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 - void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds); - void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds); + virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds); + 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);} // Converts thrust vector and heading angle to quaternion rotation in the earth frame diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp index 6bc8899a9b..82953417e5 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp @@ -48,6 +48,26 @@ void AC_AttitudeControl_Multi_6DoF::input_euler_angle_roll_pitch_yaw(float euler AC_AttitudeControl_Multi::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw); } +// 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) +{ + // convert thrust vector to a roll and pitch angles + // this negates the advantage of using thrust vector control, but works just fine + Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312(); + + input_euler_angle_roll_pitch_euler_rate_yaw(degrees(angle_target.x) * 100.0f, degrees(angle_target.y) * 100.0f, heading_rate_cds); +} + +// Command a thrust vector, heading and heading rate +void AC_AttitudeControl_Multi_6DoF::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) +{ + // convert thrust vector to a roll and pitch angles + Vector3f angle_target = attitude_from_thrust_vector(thrust_vector, _ahrs.yaw).to_vector312(); + + // note that we are throwing away heading rate here + input_euler_angle_roll_pitch_yaw(degrees(angle_target.x) * 100.0f, degrees(angle_target.y) * 100.0f, heading_angle_cd, true); +} + void AC_AttitudeControl_Multi_6DoF::set_forward_lateral(float &euler_pitch_angle_cd, float &euler_roll_angle_cd) { // pitch/forward diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h index cff16ba3f7..fcc7ba32fd 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h @@ -31,6 +31,10 @@ public: // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing 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_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds) override; + /* all other input functions should zero thrust vectoring and behave as a normal copter */