From d00824fc2bc9e0074c64e54abfbece1058d3fb74 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 12 May 2024 22:32:31 +0100 Subject: [PATCH] AC_AttitudeControl_Heli: passthrough_bf_roll_pitch_rate_yaw use quaterion to update attitude target --- .../AC_AttitudeControl_Heli.cpp | 23 ++++++++----------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 199820a070e8a..cc7d730219acc 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -364,21 +364,16 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass Quaternion att; _ahrs.get_quat_body_to_ned(att); if (ang_vel_to_euler_rate(att, _att_error_rot_vec_rad, att_error_euler_rad)) { - _euler_angle_target.x = wrap_PI(att_error_euler_rad.x + _ahrs.roll); - _euler_angle_target.y = wrap_PI(att_error_euler_rad.y + _ahrs.pitch); - _euler_angle_target.z = wrap_2PI(att_error_euler_rad.z + _ahrs.yaw); - } - // handle flipping over pitch axis - if (_euler_angle_target.y > M_PI / 2.0f) { - _euler_angle_target.x = wrap_PI(_euler_angle_target.x + M_PI); - _euler_angle_target.y = wrap_PI(M_PI - _euler_angle_target.x); - _euler_angle_target.z = wrap_2PI(_euler_angle_target.z + M_PI); - } - if (_euler_angle_target.y < -M_PI / 2.0f) { - _euler_angle_target.x = wrap_PI(_euler_angle_target.x + M_PI); - _euler_angle_target.y = wrap_PI(-M_PI - _euler_angle_target.x); - _euler_angle_target.z = wrap_2PI(_euler_angle_target.z + M_PI); + // Convert euler attitude error to Quaternion + Quaternion attitude_target_update; + attitude_target_update.from_euler(att_error_euler_rad); + + // Set target based on current attitude and error + _attitude_target = att * attitude_target_update; + + // Keep euler target upto date + _attitude_target.to_euler(_euler_angle_target); } // convert body-frame angle errors to body-frame rate targets