From a01f98d6405baf820b530eb6b18ede67dc22fd43 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 | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 6d1d45a7668a0f..bfbbd89e3a1db6 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -125,7 +125,18 @@ 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 + + // exit immediately if no angle target (this should never happen) + if (mnt_target.target_type != MountTargetType::ANGLE) { + return false; + } + + // 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; }