RC_Channel: add RETRACT_MOUNT1_3POS switch option

ArduPilot · Nov 4, 2023 · 8c60060 · 8c60060
1 parent 66b8fd9
commit 8c60060
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 5 deletions.
diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
@@ -234,6 +234,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Values{Plane}: 173:Plane AUTO Mode Landing Abort
     // @Values{Copter, Rover, Plane, Blimp}: 174:Camera Image Tracking
     // @Values{Copter, Rover, Plane, Blimp}: 175:Camera Lens
+    // @Values{Copter, Rover, Plane}: 117:Retract Mount1 3pos
     // @Values{Rover}: 201:Roll
     // @Values{Rover}: 202:Pitch
     // @Values{Rover}: 207:MainSail
@@ -692,6 +693,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo
 #if HAL_MOUNT_ENABLED
     case AUX_FUNC::RETRACT_MOUNT1:
     case AUX_FUNC::MOUNT_LOCK:
+    case AUX_FUNC::RETRACT_MOUNT1_3POS:
 #endif
     case AUX_FUNC::LOG_PAUSE:
     case AUX_FUNC::ARM_EMERGENCY_STOP:
@@ -768,6 +770,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
     { AUX_FUNC::CAMERA_AUTO_FOCUS, "Camera Auto Focus"},
     { AUX_FUNC::CAMERA_IMAGE_TRACKING, "Camera Image Tracking"},
     { AUX_FUNC::CAMERA_LENS, "Camera Lens"},
+    { AUX_FUNC::RETRACT_MOUNT1_3POS,"RetractMount1 3pos"},
 };
 
 /* lookup the announcement for switch change */
@@ -1540,6 +1543,26 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos
         mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH);
         break;
     }
+
+    case AUX_FUNC::RETRACT_MOUNT1_3POS: {
+        AP_Mount *mount = AP::mount();
+        if (mount == nullptr) {
+            break;
+        }
+        switch (ch_flag) {
+        case AuxSwitchPos::HIGH:
+            if (mount->get_mode(0) > MAV_MOUNT_MODE_NEUTRAL) _mount_mode_last = mount->get_mode(0);
+            mount->set_mode(0,MAV_MOUNT_MODE_RETRACT);
+            break;
+        case AuxSwitchPos::MIDDLE:
+            if (_mount_mode_last > MAV_MOUNT_MODE_NEUTRAL) mount->set_mode(0, _mount_mode_last);
+            break;
+        case AuxSwitchPos::LOW:
+            mount->set_mode_to_default(0);
+            break;
+        }
+        break;
+    }
 #endif
 
     case AUX_FUNC::LOG_PAUSE: {

diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
@@ -6,6 +6,7 @@
 
 #if AP_RC_CHANNEL_ENABLED
 
+#include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
@@ -77,7 +78,7 @@ class RC_Channel {
     void       set_override(const uint16_t v, const uint32_t timestamp_ms);
     bool       has_override() const;
 
-    float    stick_mixing(const float servo_in);
+    float      stick_mixing(const float servo_in);
 
     // get control input with zero deadzone
     int16_t    get_control_in_zero_dz(void) const;
@@ -212,7 +213,7 @@ class RC_Channel {
         EKF_YAW_RESET =      104, // trigger yaw reset attempt
         GPS_DISABLE_YAW =    105, // disable GPS yaw for testing
         DISABLE_AIRSPEED_USE = 106, // equivalent to AIRSPEED_USE 0
-        FW_AUTOTUNE =          107, // fixed wing auto tune
+        FW_AUTOTUNE =        107, // fixed wing auto tune
         QRTL =               108, // QRTL mode
         CUSTOM_CONTROLLER =  109,  // use Custom Controller
         KILL_IMU3 =          110, // disable third IMU (for IMU failure testing)
@@ -249,7 +250,7 @@ class RC_Channel {
         CAMERA_IMAGE_TRACKING = 174, // camera image tracking
         CAMERA_LENS =        175, // camera lens selection
         VFWD_THR_OVERRIDE =  176, // force enabled VTOL forward throttle method
-
+        RETRACT_MOUNT1_3POS = 177,// Retract Mount1 3pos
 
         // inputs from 200 will eventually used to replace RCMAP
         ROLL =               201, // roll input
@@ -422,6 +423,8 @@ class RC_Channel {
 
     static const LookupTable lookuptable[];
 #endif
+
+    enum MAV_MOUNT_MODE _mount_mode_last = MAV_MOUNT_MODE_RETRACT;
 };
 
 
@@ -460,7 +463,7 @@ class RC_Channels {
     // this function is implemented in the child class in the vehicle
     // code
     virtual RC_Channel *channel(uint8_t chan) = 0;
-    // helper used by scripting to convert the above function from 0 to 1 indexeing
+    // helper used by scripting to convert the above function from 0 to 1 indexing
     // range is checked correctly by the underlying channel function
     RC_Channel *lua_rc_channel(const uint8_t chan) {
         return channel(chan -1);
@@ -471,7 +474,7 @@ class RC_Channels {
 
     static uint8_t get_valid_channel_count(void);                      // returns the number of valid channels in the last read
     static int16_t get_receiver_rssi(void);                            // returns [0, 255] for receiver RSSI (0 is no link) if present, otherwise -1
-    static int16_t get_receiver_link_quality(void);                         // returns 0-100 % of last 100 packets received at receiver are valid
+    static int16_t get_receiver_link_quality(void);                    // returns 0-100 % of last 100 packets received at receiver are valid
     bool read_input(void);                                             // returns true if new input has been read in
     static void clear_overrides(void);                                 // clears any active overrides
     static bool receiver_bind(const int dsmMode);                      // puts the receiver in bind mode if present, returns true if success