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

AC_AttitudeControl: Fix dt update order

This commit is contained in:
Leonard Hall 2024-09-10 20:41:51 +09:30 committed by Andrew Tridgell
parent b1b4ac8d58
commit 5e27e3111d
2 changed files with 45 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -256,14 +256,15 @@ void AC_AttitudeControl::landed_gain_reduction(bool landed)
// remain very close together.
//
// All input functions below follow the same procedure
// 1. define the desired attitude the aircraft should attempt to achieve using the input variables
// 2. using the desired attitude and input variables, define the target angular velocity so that it should
// 1. define the desired attitude or attitude change based on the input variables
// 2. update the target attitude based on the angular velocity target and the time since the last loop
// 3. using the desired attitude and input variables, define the target angular velocity so that it should
// move the target attitude towards the desired attitude
// 3. if _rate_bf_ff_enabled is not being used then make the target attitude
// 4. if _rate_bf_ff_enabled is not being used then make the target attitude
// and target angular velocities equal to the desired attitude and desired angular velocities.
// 4. ensure _attitude_target, _euler_angle_target, _euler_rate_target and
// 5. ensure _attitude_target, _euler_angle_target, _euler_rate_target and
// _ang_vel_target have been defined. This ensures input modes can be changed without discontinuity.
// 5. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and
// 6. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and
// integrate them into the target attitude. Any errors between the target attitude and the measured attitude are
// corrected by first correcting the thrust vector until the angle between the target thrust vector measured
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
@ -272,15 +273,18 @@ void AC_AttitudeControl::landed_gain_reduction(bool landed)
// 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);
// update attitude target
update_attitude_target();
// Limit the angular velocity
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) {
Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by _input_tc at the end.
@ -316,6 +320,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
// update attitude target
update_attitude_target();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
@ -367,6 +374,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f);
// update attitude target
update_attitude_target();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
@ -426,6 +436,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
float euler_pitch_rate = radians(euler_pitch_rate_cds * 0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
// update attitude target
update_attitude_target();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
@ -468,6 +481,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
// update attitude target
update_attitude_target();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
@ -543,6 +559,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
Vector3f gyro_latest = _ahrs.get_gyro_latest();
attitude_ang_error_update_quat.from_axis_angle(Vector3f{(_ang_vel_target.x-gyro_latest.x) * _dt, (_ang_vel_target.y-gyro_latest.y) * _dt, (_ang_vel_target.z-gyro_latest.z) * _dt});
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
_attitude_ang_error.normalize();
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
@ -557,6 +574,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
// Update the unused targets attitude based on current attitude to condition mode change
_attitude_target = attitude_body * _attitude_ang_error;
_attitude_target.normalize();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
@ -568,9 +586,6 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
_attitude_ang_error.to_axis_angle(attitude_error);
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
_ang_vel_body += _ang_vel_target;
// ensure Quaternions stay normalized
_attitude_ang_error.normalize();
}
// Command an angular step (i.e change) in body frame angle
@ -610,6 +625,9 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
heading_rate = constrain_float(heading_rate, -slew_yaw_max_rads, slew_yaw_max_rads);
}
// update attitude target
update_attitude_target();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
@ -662,6 +680,9 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect
float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -slew_yaw_max_rads, slew_yaw_max_rads);
float heading_angle = radians(heading_angle_cd * 0.01f);
// update attitude target
update_attitude_target();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
@ -747,6 +768,16 @@ Quaternion AC_AttitudeControl::attitude_from_thrust_vector(Vector3f thrust_vecto
return thrust_vec_quat*yaw_quat;
}
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::update_attitude_target()
{
// rotate target and normalize
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(_ang_vel_target * _dt);
_attitude_target *= attitude_target_update;
_attitude_target.normalize();
}
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
@ -784,16 +815,6 @@ void AC_AttitudeControl::attitude_controller_run_quat()
_ang_vel_body += ang_vel_body_feedforward;
}
if (_rate_bf_ff_enabled) {
// rotate target and normalize
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt});
_attitude_target = _attitude_target * attitude_target_update;
}
// ensure Quaternion stay normalised
_attitude_target.normalize();
// Record error to handle EKF resets
_attitude_ang_error = attitude_body.inverse() * _attitude_target;
}

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -346,6 +346,9 @@ public:
// translates body frame acceleration limits to the euler axis
Vector3f euler_accel_limit(const Quaternion &att, const Vector3f &euler_accel);
// Calculates the body frame angular velocities to follow the target attitude
void update_attitude_target();
// Calculates the body frame angular velocities to follow the target attitude
void attitude_controller_run_quat();