From 10da8a42bd5f9c1959bef8788fec4138b6d87d53 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 16 Jun 2022 16:30:01 +0900
Subject: [PATCH] AP_Mount: replace JSTICK_SPD with RC_RATE

---
 libraries/AP_Mount/AP_Mount.cpp         | 32 ++++++++++++++++++-------
 libraries/AP_Mount/AP_Mount.h           |  5 +++-
 libraries/AP_Mount/AP_Mount_Backend.cpp |  4 ++--
 3 files changed, 30 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 818b76ed9d..222dfcc230 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -179,13 +179,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_ANGMAX_PAN",  15, AP_Mount, state[0]._pan_angle_max,  4500),
 
-    // @Param: _JSTICK_SPD
-    // @DisplayName: mount joystick speed
-    // @Description: 0 for position control, small for low speeds, 100 for max speed. A good general value is 10 which gives a movement speed of 3 degrees per second.
-    // @Range: 0 100
-    // @Increment: 1
-    // @User: Standard
-    AP_GROUPINFO("_JSTICK_SPD",  16, AP_Mount, _joystick_speed, 0),
+    // 16 was _JSTICK_SPD
 
     // @Param: _LEAD_RLL
     // @DisplayName: Roll stabilization lead time
@@ -215,7 +209,14 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
 
     // 23 formerly _K_RATE
 
-    // 24 is AVAILABLE
+    // @Param: _RC_RATE
+    // @DisplayName: Mount RC Rate
+    // @Description: Pilot rate control's maximum rate.  Set to zero to use angle control
+    // @Units: deg/s
+    // @Range: 0 90
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("_RC_RATE",  24, AP_Mount, _rc_rate_max, 0),
 
 #if AP_MOUNT_MAX_INSTANCES > 1
     // @Param: 2_DEFLT_MODE
@@ -432,6 +433,9 @@ void AP_Mount::init()
         }
     }
 
+    // perform any required parameter conversion
+    convert_params();
+
     // primary is reset to the first instantiated mount
     bool primary_set = false;
 
@@ -828,6 +832,18 @@ void AP_Mount::handle_gimbal_device_attitude_status(const mavlink_message_t &msg
     }
 }
 
+// perform any required parameter conversion
+void AP_Mount::convert_params()
+{
+    // convert JSTICK_SPD to RC_RATE
+    if (!_rc_rate_max.configured()) {
+        int8_t jstick_spd = 0;
+        if (AP_Param::get_param_by_index(this, 16, AP_PARAM_INT8, &jstick_spd) && (jstick_spd > 0)) {
+            _rc_rate_max.set_and_save(jstick_spd * 0.3);
+        }
+    }
+}
+
 // singleton instance
 AP_Mount *AP_Mount::_singleton;
 
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 0d7ab95c3a..42d9a39a8e 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -151,7 +151,7 @@ protected:
     static AP_Mount *_singleton;
 
     // frontend parameters
-    AP_Int8             _joystick_speed;    // joystick gain
+    AP_Int16            _rc_rate_max;       // Pilot rate control's maximum rate.  Set to zero to use angle control
 
     // front end members
     uint8_t             _num_instances;     // number of mounts instantiated
@@ -212,6 +212,9 @@ private:
     void handle_global_position_int(const mavlink_message_t &msg);
     void handle_gimbal_device_information(const mavlink_message_t &msg);
     void handle_gimbal_device_attitude_status(const mavlink_message_t &msg);
+
+    // perform any required parameter conversion
+    void convert_params();
 };
 
 namespace AP {
diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
index 271dccf8aa..a5d0c4844e 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.cpp
+++ b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -125,7 +125,7 @@ void AP_Mount_Backend::update_rate_and_angle_from_rc(const RC_Channel *chan, flo
         rate_rads = 0;
         return;
     }
-    rate_rads = chan->norm_input_dz() * 0.005f * _frontend._joystick_speed;
+    rate_rads = chan->norm_input_dz() * radians(_frontend._rc_rate_max);
     angle_rad = constrain_float(angle_rad + (rate_rads * AP_MOUNT_UPDATE_DT), radians(angle_min_rad*0.01f), radians(angle_max_rad*0.01f));
 }
 
@@ -137,7 +137,7 @@ void AP_Mount_Backend::update_targets_from_rc()
     const RC_Channel *pan_ch = rc().channel(_state._pan_rc_in - 1);
 
     // if joystick_speed is defined then pilot input defines a rate of change of the angle
-    if (_frontend._joystick_speed > 0) {
+    if (_frontend._rc_rate_max > 0) {
         // allow pilot position input to come directly from an RC_Channel
         update_rate_and_angle_from_rc(roll_ch, _rate_target_rads.x, _angle_ef_target_rad.x, _state._roll_angle_min, _state._roll_angle_max);
         update_rate_and_angle_from_rc(tilt_ch, _rate_target_rads.y, _angle_ef_target_rad.y, _state._tilt_angle_min, _state._tilt_angle_max);