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.
// 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

4
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -160,8 +160,8 @@ public:
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);

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
// 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 +156,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);
}

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
View File

@ -20,8 +20,7 @@ public:
// 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 so this implementation 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 offset angles
*/