RC_Channel: Add a MOUNT2 retract

muramura · Jun 21, 2024 · 175f5a7 · 175f5a7
1 parent 215f71a
commit 175f5a7
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
@@ -209,6 +209,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Values{Copter}: 109:use Custom Controller
     // @Values{Copter, Rover, Plane, Blimp}:  110:KillIMU3
     // @Values{Copter,Plane,Rover,Blimp,Sub,Tracker}: 112:SwitchExternalAHRS
+    // @Values{Copter, Rover, Plane}: 113:Retract Mount2
     // @Values{Plane}: 150:CRUISE Mode
     // @Values{Copter}: 151:TURTLE Mode
     // @Values{Copter}: 152:SIMPLE heading reset
@@ -699,6 +700,7 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos
     case AUX_FUNC::FFT_NOTCH_TUNE:
 #if HAL_MOUNT_ENABLED
     case AUX_FUNC::RETRACT_MOUNT1:
+    case AUX_FUNC::RETRACT_MOUNT2:
     case AUX_FUNC::MOUNT_LOCK:
 #endif
     case AUX_FUNC::LOG_PAUSE:
@@ -735,6 +737,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
     { AUX_FUNC::PARACHUTE_3POS,"Parachute3Position"},
     { AUX_FUNC::MISSION_RESET,"MissionReset"},
     { AUX_FUNC::RETRACT_MOUNT1,"RetractMount1"},
+    { AUX_FUNC::RETRACT_MOUNT2,"RetractMount2"},
     { AUX_FUNC::RELAY,"Relay1"},
     { AUX_FUNC::MOTOR_ESTOP,"MotorEStop"},
     { AUX_FUNC::MOTOR_INTERLOCK,"MotorInterlock"},
@@ -1589,6 +1592,25 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch
         break;
     }
 
+    case AUX_FUNC::RETRACT_MOUNT2: {
+        AP_Mount *mount = AP::mount();
+        if (mount == nullptr || mount->get_num_instances() < 2u) {
+            break;
+        }
+        switch (ch_flag) {
+        case AuxSwitchPos::HIGH:
+            mount->set_mode(1,MAV_MOUNT_MODE_RETRACT);
+            break;
+        case AuxSwitchPos::MIDDLE:
+            // nothing
+            break;
+        case AuxSwitchPos::LOW:
+            mount->set_mode_to_default(1);
+            break;
+        }
+        break;
+    }
+
     case AUX_FUNC::MOUNT_LOCK: {
         AP_Mount *mount = AP::mount();
         if (mount == nullptr) {

diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
@@ -217,6 +217,7 @@ class RC_Channel {
         KILL_IMU3 =          110, // disable third IMU (for IMU failure testing)
         LOWEHEISER_STARTER = 111,  // allows for manually running starter
         AHRS_TYPE =          112, // change AHRS_EKF_TYPE
+        RETRACT_MOUNT2 =     113, // Retract Mount2
 
         // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp!
         // also, if you add an option >255, you will need to fix duplicate_options_exist