AP_Mount: add get_attitude_euler

this allows external callers including Lua to retrieve the gimbal mount's current attitude

libraries/AP_Mount/AP_Mount.cpp

@@ -379,6 +379,18 @@ void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan)
     }
 }
 
+// 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)
+{
+    if (!check_instance(instance)) {
+        return false;
+    }
+
+    // send command to backend
+    return _backends[instance]->get_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg);
+}
+
 // run pre-arm check.  returns false on failure and fills in failure_msg
 // any failure_msg returned will not include a prefix
 bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)

libraries/AP_Mount/AP_Mount.h

@@ -154,6 +154,10 @@ public:
     // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
     void send_gimbal_device_attitude_status(mavlink_channel_t chan);
 
+    // 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);
+
     // run pre-arm check.  returns false on failure and fills in failure_msg
     // any failure_msg returned will not include a prefix
     bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -7,6 +7,24 @@ extern const AP_HAL::HAL& hal;
 
 #define AP_MOUNT_UPDATE_DT 0.02     // update rate in seconds.  update() should be called at this rate
 
+// get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
+// returns true on success
+bool AP_Mount_Backend::get_attitude_euler(float& roll_deg, float& pitch_deg, float& yaw_bf_deg)
+{
+    // by default re-use get_attitude_quaternion and convert to Euler angles
+    Quaternion att_quat;
+    if (!get_attitude_quaternion(att_quat)) {
+        return false;
+    }
+
+    float roll_rad, pitch_rad, yaw_rad;
+    att_quat.to_euler(roll_rad, pitch_rad, yaw_rad);
+    roll_deg = degrees(roll_rad);
+    pitch_deg = degrees(pitch_rad);
+    yaw_bf_deg = degrees(yaw_rad);
+    return true;
+}
+
 // set angle target in degrees
 // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
 void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)

libraries/AP_Mount/AP_Mount_Backend.h

@@ -49,6 +49,10 @@ public:
     // returns true if this mount can control its pan (required for multicopters)
     virtual bool has_pan_control() const = 0;
 
+    // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
+    // returns true on success
+    bool get_attitude_euler(float& roll_deg, float& pitch_deg, float& yaw_bf_deg);
+
     // get mount's mode
     enum MAV_MOUNT_MODE get_mode() const { return _mode; }