From 5a1a8d1c108b4c962c6bdc87d4e4169ae6c350f2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 6 Dec 2024 20:42:58 +0900
Subject: [PATCH] AP_Mount: add get_attitude_quaternion method

---
 libraries/AP_Mount/AP_Mount.cpp | 15 +++++++++++----
 libraries/AP_Mount/AP_Mount.h   |  5 +++++
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 8dc31bbfa3..0b64b1063b 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -655,18 +655,25 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati
 }
 #endif
 
-// get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
-// returns true on success
-bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)
+// get attitude as a quaternion.  returns true on success.
+// att_quat will be an earth-frame quaternion rotated such that
+// yaw is in body-frame.
+bool AP_Mount::get_attitude_quaternion(uint8_t instance, Quaternion& att_quat)
 {
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
     }
+    return backend->get_attitude_quaternion(att_quat);
+}
 
+// get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
+// returns true on success
+bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)
+{
     // re-use get_attitude_quaternion and convert to Euler angles
     Quaternion att_quat;
-    if (!backend->get_attitude_quaternion(att_quat)) {
+    if (!get_attitude_quaternion(instance, att_quat)) {
         return false;
     }
 
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 54762e9153..d34f581636 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -206,6 +206,11 @@ public:
     bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const;
 #endif
 
+    // get attitude as a quaternion.  returns true on success.
+    // att_quat will be an earth-frame quaternion rotated such that
+    // yaw is in body-frame.
+    bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat);
+
     // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
     // returns true on success
     bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);