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

AC_AttitudeControl: Vector3f multiplication clean up

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

4
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -500,7 +500,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
} else { } else {
// When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed. // When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
Quaternion attitude_target_update; Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f{roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt}); attitude_target_update.from_axis_angle(Vector3f{roll_rate_rads, pitch_rate_rads, yaw_rate_rads} * _dt);
_attitude_target = _attitude_target * attitude_target_update; _attitude_target = _attitude_target * attitude_target_update;
_attitude_target.normalize(); _attitude_target.normalize();
@ -557,7 +557,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
} }
Vector3f gyro_latest = _ahrs.get_gyro_latest(); 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_update_quat.from_axis_angle((_ang_vel_target - gyro_latest) * _dt);
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error; _attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
_attitude_ang_error.normalize(); _attitude_ang_error.normalize();