Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: Change input_quaternion to use body frame rates

This commit is contained in:
Leonard Hall 2023-08-10 11:27:00 +09:30 committed by Peter Barker
parent 72a1b97b4c
commit 7047e11090
4 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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. // 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 // Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step by the integral of the angular velocity // 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_target) void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body)
{ {
Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat; Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle; Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle); attitude_error_quat.to_axis_angle(attitude_error_angle);
// Limit the angular velocity // 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) { if (_rate_bf_ff_enabled) {
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler

4
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -160,8 +160,8 @@ public:
void inertial_frame_reset(); void inertial_frame_reset();
// Command a Quaternion attitude with feedforward and smoothing // 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
virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target); 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 // 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); 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);

5
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp
View File

@ -148,8 +148,7 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol
// Command a Quaternion attitude with feedforward and smoothing // 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 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_body) {
void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF"); AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
#endif #endif
@ -157,7 +156,7 @@ void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desire
_motors.set_lateral(0.0f); _motors.set_lateral(0.0f);
_motors.set_forward(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);
} }

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
View File

@ -20,8 +20,7 @@ public:
// Command a Quaternion attitude with feedforward and smoothing // 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 angular velocity
// not used anywhere in current code, panic so this implementation is not overlooked void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) override;
void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override;
/* /*
override input functions to attitude controller and convert desired angles into thrust angles and substitute for offset angles override input functions to attitude controller and convert desired angles into thrust angles and substitute for offset angles
*/ */