From 4755a60863e52f5388fcb0a9b02e8a39099a0428 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 19 Mar 2024 19:32:45 +0900
Subject: [PATCH] AP_Mount: RC targeting mode lock state from previous mode

This leads to smoother transitions between RC and GCS control
---
 libraries/AP_Mount/AP_Mount_Backend.cpp | 20 ++++++++++++++++++++
 libraries/AP_Mount/AP_Mount_Backend.h   |  6 ++++++
 libraries/AP_Mount/AP_Mount_Params.cpp  |  7 +++++++
 libraries/AP_Mount/AP_Mount_Params.h    |  1 +
 4 files changed, 34 insertions(+)

diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
index 9e15e719ab..b43de69dee 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.cpp
+++ b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -91,6 +91,11 @@ void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float y
 
     // set the mode to mavlink targeting
     set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(yaw_is_earth_frame);
+    }
 }
 
 // sets rate target in deg/s
@@ -106,6 +111,11 @@ void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float
 
     // set the mode to mavlink targeting
     set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(yaw_is_earth_frame);
+    }
 }
 
 // set_roi_target - sets target location that mount should attempt to point towards
@@ -117,6 +127,11 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc)
 
     // set the mode to GPS tracking mode
     set_mode(MAV_MOUNT_MODE_GPS_POINT);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(true);
+    }
 }
 
 // clear_roi_target - clears target location that mount should attempt to point towards
@@ -139,6 +154,11 @@ void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
 
     // set the mode to sysid tracking mode
     set_mode(MAV_MOUNT_MODE_SYSID_TARGET);
+
+    // optionally set RC_TARGETING yaw lock state
+    if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
+        set_yaw_lock(true);
+    }
 }
 
 #if AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index 07f12a203a..5198cd6690 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -237,6 +237,12 @@ protected:
         void set(const Vector3f& rpy, bool yaw_is_ef_in);
     };
 
+    // options parameter bitmask handling
+    enum class Options : uint8_t {
+        RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode
+    };
+    bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; }
+
     // returns true if user has configured a valid yaw angle range
     // allows user to disable yaw even on 3-axis gimbal
     bool yaw_range_valid() const { return (_params.yaw_angle_min < _params.yaw_angle_max); }
diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp
index c811afde84..57257eb5e4 100644
--- a/libraries/AP_Mount/AP_Mount_Params.cpp
+++ b/libraries/AP_Mount/AP_Mount_Params.cpp
@@ -165,6 +165,13 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO_FLAGS("_DEVID", 15, AP_Mount_Params, dev_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
 
+    // @Param: _OPTIONS
+    // @DisplayName: Mount options
+    // @Description: Mount options bitmask
+    // @Bitmask: 0:RC lock state from previous mode
+    // @User: Standard
+    AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0),
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_Mount/AP_Mount_Params.h b/libraries/AP_Mount/AP_Mount_Params.h
index a3d7e12b01..ccf42c1fe0 100644
--- a/libraries/AP_Mount/AP_Mount_Params.h
+++ b/libraries/AP_Mount/AP_Mount_Params.h
@@ -31,4 +31,5 @@ public:
     AP_Float    pitch_stb_lead;     // pitch lead control gain (only used by servo backend)
     AP_Int8     sysid_default;      // target sysid for mount to follow
     AP_Int32    dev_id;             // Device id taking into account bus
+    AP_Int8     options;            // mount options bitmask
 };