From 8b5539c055ca002b3bd75ed075e22c59e3c52ea2 Mon Sep 17 00:00:00 2001
From: Nick Exton <NExton@sypaq.com.au>
Date: Thu, 16 May 2024 16:57:25 +1000
Subject: [PATCH] AP_Mount: Clarify angle frames of set_angle_target()

---
 libraries/AP_Mount/AP_Mount.cpp         |  1 +
 libraries/AP_Mount/AP_Mount.h           |  1 +
 libraries/AP_Mount/AP_Mount_Backend.cpp | 13 +++++++------
 libraries/AP_Mount/AP_Mount_Backend.h   |  1 +
 4 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 3bfed9c292..70a136283d 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -263,6 +263,7 @@ void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock)
 }
 
 // set angle target in degrees
+// roll and pitch are in earth-frame
 // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
 void AP_Mount::set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)
 {
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index d67526302a..110267bcc4 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -157,6 +157,7 @@ public:
     void set_yaw_lock(uint8_t instance, bool yaw_lock);
 
     // set angle target in degrees
+    // roll and pitch are in earth-frame
     // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
     void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) { set_angle_target(_primary, roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); }
     void set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);
diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
index 5fc2a3cff0..9d064633b1 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.cpp
+++ b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -67,6 +67,7 @@ bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode)
 }
 
 // set angle target in degrees
+// roll and pitch are in earth-frame
 // 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)
 {
@@ -277,8 +278,8 @@ void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packe
 {
     switch (get_mode()) {
     case MAV_MOUNT_MODE_MAVLINK_TARGETING:
-        // input_a : Pitch in centi-degrees
-        // input_b : Roll in centi-degrees
+        // input_a : Pitch in centi-degrees (earth-frame)
+        // input_b : Roll in centi-degrees (earth-frame)
         // input_c : Yaw in centi-degrees (interpreted as body-frame)
         set_angle_target(packet.input_b * 0.01, packet.input_a * 0.01, packet.input_c * 0.01, false);
         break;
@@ -325,10 +326,10 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_comma
         return MAV_RESULT_ACCEPTED;
 
     case MAV_MOUNT_MODE_MAVLINK_TARGETING: {
-        // set body-frame target angles (in degrees) from mavlink message
-        const float pitch_deg = packet.param1;  // param1: pitch (in degrees)
-        const float roll_deg = packet.param2;   // param2: roll in degrees
-        const float yaw_deg = packet.param3;    // param3: yaw in degrees
+        // set target angles (in degrees) from mavlink message
+        const float pitch_deg = packet.param1;  // param1: pitch (earth-frame, degrees)
+        const float roll_deg = packet.param2;   // param2: roll (earth-frame, degrees)
+        const float yaw_deg = packet.param3;    // param3: yaw (body-frame, degrees)
 
         // warn if angles are invalid to catch angles sent in centi-degrees
         if ((fabsf(pitch_deg) > 90) || (fabsf(roll_deg) > 180) || (fabsf(yaw_deg) > 360)) {
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index 56706d7463..39a784ddb5 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -84,6 +84,7 @@ public:
     void set_yaw_lock(bool yaw_lock) { _yaw_lock = yaw_lock; }
 
     // set angle target in degrees
+    // roll and pitch are in earth-frame
     // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
     void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);