From e3e111ae8bf5bdc30fcf2bb1259a94d688da5cfe Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 14 Oct 2023 19:52:24 +0900 Subject: [PATCH] AP_Mount: Servo get_attitude_quat fix --- libraries/AP_Mount/AP_Mount_Servo.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 6d1d45a766..196b79edff 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -125,7 +125,14 @@ 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) { - att_quat.from_euler(_angle_bf_output_rad); + // 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)); + + // convert to quaternion + att_quat.from_euler(Vector3f{mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, yaw_bf}); return true; }