From e29924d5fe9c3ca6b1ed1b229a045c032da2f4dc Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 10 Aug 2023 11:27:00 +0930 Subject: [PATCH] AC_AttitudeControl: Change input_quaternion to use body frame rates --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 7 ++++--- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 4 ++-- .../AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp | 4 ++-- .../AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h | 4 ++-- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index a64a277d883cfd..672b53765d81f7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -227,15 +227,16 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() // trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected. // Command a Quaternion attitude with feedforward and smoothing -// attitude_desired_quat: is updated on each time_step by the integral of the angular velocity -void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) +// attitude_desired_quat: is updated on each time_step by the integral of the body frame angular velocity +void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) { Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat; Vector3f attitude_error_angle; attitude_error_quat.to_axis_angle(attitude_error_angle); // Limit the angular velocity - ang_vel_limit(ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); + ang_vel_limit(ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); + Vector3f ang_vel_target = attitude_desired_quat * ang_vel_body; if (_rate_bf_ff_enabled) { // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index b9edc4a1708bde..2159f8e7bed03a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -159,8 +159,8 @@ class AC_AttitudeControl { void inertial_frame_reset(); // Command a Quaternion attitude with feedforward and smoothing - // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity - virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target); + // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity + virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body); // Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp index ade5317e2a9787..59b5dbf72548bf 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp @@ -149,7 +149,7 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol // Command a Quaternion attitude with feedforward and smoothing // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity // not used anywhere in current code, panic in SITL so this implementation is not overlooked -void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) { +void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF"); #endif @@ -157,7 +157,7 @@ void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desire _motors.set_lateral(0.0f); _motors.set_forward(0.0f); - AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_target); + AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_body); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h index f089f01d4669da..e067231204346f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h @@ -19,9 +19,9 @@ class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi { } // Command a Quaternion attitude with feedforward and smoothing - // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity + // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity // not used anywhere in current code, panic so this implementaiton is not overlooked - void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override; + void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) override; /* override input functions to attitude controller and convert desired angles into thrust angles and substitute for osset angles */