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

AP_Mount: In Servo backend, report attitude as demanded servo angles, not target angles

This commit is contained in:
Nick Exton 2024-05-16 12:15:16 +10:00 committed by Randy Mackay
parent e17439287b
commit 0af4649705
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -140,14 +140,23 @@ bool AP_Mount_Servo::has_pan_control() const
// get attitude as a quaternion. returns true on success
bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat)
{
// no feedback from gimbal so simply report targets
// mnt_target.angle_rad always holds latest angle targets
// ensure yaw target is in body-frame with limits applied
const float yaw_bf = constrain_float(mnt_target.angle_rad.get_bf_yaw(), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
// No feedback from gimbal so simply report demanded servo angles (which is
// not the same as target angles).
float roll_rad = 0.0f;
float pitch_rad = 0.0f;
float yaw_rad = 0.0f;
if (has_roll_control()) {
roll_rad = constrain_float(_angle_bf_output_rad.x, radians(_params.roll_angle_min), radians(_params.roll_angle_max));
}
if (has_pitch_control()) {
pitch_rad = constrain_float(_angle_bf_output_rad.y, radians(_params.pitch_angle_min), radians(_params.pitch_angle_max));
}
if (has_pan_control()) {
yaw_rad = constrain_float(_angle_bf_output_rad.z, radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
}
// convert to quaternion
att_quat.from_euler(Vector3f{mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, yaw_bf});
att_quat.from_euler(roll_rad, pitch_rad, yaw_rad);
return true;
}