From ea226637c90f7e4d1c606b5d66704acda7897f1f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 10 Sep 2024 20:42:28 +0930 Subject: [PATCH] AC_AttitudeControl: Vector3f multiplication clean up --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 70db1a2a1b..a42e031af0 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -500,7 +500,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl } else { // 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; - 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.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(); - 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.normalize();