Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl_Heli: passthrough_bf_roll_pitch_rate_yaw use quaterion to update attitude target

Browse Source
This commit is contained in:
Iampete1 2024-05-12 22:32:31 +01:00
parent fbb785859a
commit d00824fc2b
1 changed files with 9 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -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