wip

Williangalvani · Dec 5, 2024 · aec3118 · aec3118
1 parent ad539ff
commit aec3118
Show file tree

Hide file tree

Showing 4 changed files with 142 additions and 6 deletions.
diff --git a/ArduSub/RC_Channel.cpp b/ArduSub/RC_Channel.cpp
@@ -10,8 +10,130 @@
 
 #include <RC_Channel/RC_Channels_VarInfo.h>
 
-// note that this callback is not presently used on Plane:
-int8_t RC_Channels_Sub::flight_mode_channel_number() const
+// init_aux_switch_function - initialize aux functions
+void RC_Channel_Sub::init_aux_function(aux_func_t ch_option, AuxSwitchPos ch_flag)
 {
-    return 1; // sub does not have a flight mode channel
+    // init channel options
+    switch(ch_option) {
+    // the following functions do not need to be initialised:
+    case AUX_FUNC::ALTHOLD:
+    case AUX_FUNC::AUTO:
+    case AUX_FUNC::AUTOTUNE:
+    case AUX_FUNC::BRAKE:
+    case AUX_FUNC::CIRCLE:
+    case AUX_FUNC::DRIFT:
+    case AUX_FUNC::FLIP:
+    case AUX_FUNC::FLOWHOLD:
+    case AUX_FUNC::FOLLOW:
+    case AUX_FUNC::GUIDED:
+    case AUX_FUNC::LAND:
+    case AUX_FUNC::LOITER:
+    case AUX_FUNC::PARACHUTE_RELEASE:
+    case AUX_FUNC::POSHOLD:
+    case AUX_FUNC::RESETTOARMEDYAW:
+    case AUX_FUNC::RTL:
+    case AUX_FUNC::SAVE_TRIM:
+    case AUX_FUNC::SAVE_WP:
+    case AUX_FUNC::SMART_RTL:
+    case AUX_FUNC::STABILIZE:
+    case AUX_FUNC::THROW:
+    case AUX_FUNC::USER_FUNC1:
+    case AUX_FUNC::USER_FUNC2:
+    case AUX_FUNC::USER_FUNC3:
+    case AUX_FUNC::WINCH_CONTROL:
+    case AUX_FUNC::ZIGZAG:
+    case AUX_FUNC::ZIGZAG_Auto:
+    case AUX_FUNC::ZIGZAG_SaveWP:
+    case AUX_FUNC::ACRO:
+    case AUX_FUNC::AUTO_RTL:
+    case AUX_FUNC::TURTLE:
+    case AUX_FUNC::SIMPLE_HEADING_RESET:
+    case AUX_FUNC::ARMDISARM_AIRMODE:
+    case AUX_FUNC::TURBINE_START:
+        break;
+    case AUX_FUNC::ACRO_TRAINER:
+    case AUX_FUNC::ATTCON_ACCEL_LIM:
+    case AUX_FUNC::ATTCON_FEEDFWD:
+    case AUX_FUNC::INVERTED:
+    case AUX_FUNC::MOTOR_INTERLOCK:
+    case AUX_FUNC::PARACHUTE_3POS:      // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
+    case AUX_FUNC::PARACHUTE_ENABLE:
+    case AUX_FUNC::PRECISION_LOITER:
+    case AUX_FUNC::RANGEFINDER:
+    case AUX_FUNC::SIMPLE_MODE:
+    case AUX_FUNC::STANDBY:
+    case AUX_FUNC::SUPERSIMPLE_MODE:
+    case AUX_FUNC::SURFACE_TRACKING:
+    case AUX_FUNC::WINCH_ENABLE:
+    case AUX_FUNC::AIRMODE:
+    case AUX_FUNC::FORCEFLYING:
+    case AUX_FUNC::CUSTOM_CONTROLLER:
+    case AUX_FUNC::WEATHER_VANE_ENABLE:
+        run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
+        break;
+    default:
+        RC_Channel::init_aux_function(ch_option, ch_flag);
+        break;
+    }
+}
+
+
+// do_aux_function - implement the function invoked by auxiliary switches
+bool RC_Channel_Sub::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
+{
+    switch(ch_option) {
+        case AUX_FUNC::AUTO:
+            do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
+            break;
+        case AUX_FUNC::GUIDED:
+            do_aux_function_change_mode(Mode::Number::GUIDED, ch_flag);
+            break;
+
+        case AUX_FUNC::LOITER:
+            do_aux_function_change_mode(Mode::Number::POSHOLD, ch_flag);
+            break;
+
+        case AUX_FUNC::STABILIZE:
+            do_aux_function_change_mode(Mode::Number::STABILIZE, ch_flag);
+            break;
+
+        case AUX_FUNC::POSHOLD:
+            do_aux_function_change_mode(Mode::Number::POSHOLD, ch_flag);
+            break;
+
+        case AUX_FUNC::ALTHOLD:
+            do_aux_function_change_mode(Mode::Number::ALT_HOLD, ch_flag);
+            break;
+
+        case AUX_FUNC::ACRO:
+            do_aux_function_change_mode(Mode::Number::ACRO, ch_flag);
+            break;
+
+        case AUX_FUNC::CIRCLE:
+            do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
+            break;
+
+        case AUX_FUNC::SURFACE_TRACKING:
+            break;
+
+    default:
+        return RC_Channel::do_aux_function(ch_option, ch_flag);
+    }
+    return true;
+}
+
+// do_aux_function_change_mode - change mode based on an aux switch
+// being moved
+void RC_Channel_Sub::do_aux_function_change_mode(const Mode::Number mode,
+                                                    const AuxSwitchPos ch_flag)
+{
+    switch(ch_flag) {
+    case AuxSwitchPos::HIGH: {
+        // engage mode (if not possible we remain in current flight mode)
+        sub.set_mode(mode, ModeReason::RC_COMMAND);
+        break;
+    }
+    default:
+        break;
+    }
 }
diff --git a/ArduSub/RC_Channel.h b/ArduSub/RC_Channel.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <RC_Channel/RC_Channel.h>
+#include "mode.h"
 
 class RC_Channel_Sub : public RC_Channel
 {
@@ -9,8 +10,12 @@ class RC_Channel_Sub : public RC_Channel
 
 protected:
 
-private:
+    void init_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag) override;
+    bool do_aux_function(aux_func_t ch_option, AuxSwitchPos) override;
 
+private:
+    void do_aux_function_change_mode(const Mode::Number mode,
+                                     const AuxSwitchPos ch_flag);
 };
 
 class RC_Channels_Sub : public RC_Channels
@@ -31,6 +36,6 @@ class RC_Channels_Sub : public RC_Channels
 protected:
 
     // note that these callbacks are not presently used on Plane:
-    int8_t flight_mode_channel_number() const override;
+    int8_t flight_mode_channel_number() const override {return 0;};
 
 };
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
@@ -106,6 +106,7 @@ class Sub : public AP_Vehicle {
     friend class ParametersG2;
     friend class AP_Arming_Sub;
     friend class RC_Channels_Sub;
+    friend class RC_Channel_Sub;
     friend class Mode;
     friend class ModeManual;
     friend class ModeStabilize;

diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
@@ -295,8 +295,16 @@ class RC_Channel {
         SCRIPTING_7 =        306,
         SCRIPTING_8 =        307,
 
+        // Sub-specific functions
+        LIGHTS1_INC =        308, // increment lights1 brightness
+        LIGHTS1_DEC =        309, // decrement lights1 brightness
+        LIGHTS1_CYCLE =      310, // cycle lights1 brightness
+        LIGHTS2_INC =        311, // increment lights2 brightness
+        LIGHTS2_DEC =        312, // decrement lights2 brightness
+        LIGHTS2_CYCLE =      313, // cycle lights2 brightness
+
         // this must be higher than any aux function above
-        AUX_FUNCTION_MAX =   308,
+        AUX_FUNCTION_MAX =   314,
     };
 
     // auxiliary switch handling (n.b.: we store this as 2-bits!):