diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 263cbaa027..759088524a 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -52,6 +52,15 @@ ManualControl::~ManualControl() bool ManualControl::init() { +#if defined(PAYLOAD_POWER_EN) + + // If the payload power switch is mapped, default to power off until the RC switch explicitly commands it on. + if (_param_rc_map_pay_sw.get()) { + PAYLOAD_POWER_EN(false); + } + +#endif // PAYLOAD_POWER_EN + ScheduleNow(); return true; } @@ -293,6 +302,18 @@ void ManualControl::processSwitches(hrt_abstime &now) } else if (!_armed) { // Directly initialize mode using RC switch but only before arming evaluateModeSlot(switches.mode_slot); +#if defined(PAYLOAD_POWER_EN) + + // Apply payload power state on first switch receipt if not armed + if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_ON) { + PAYLOAD_POWER_EN(true); + + } else if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_OFF + || switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_MIDDLE) { + PAYLOAD_POWER_EN(false); + } + +#endif // PAYLOAD_POWER_EN } _previous_switches = switches; diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 42856bb650..fd01cbdab0 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -149,6 +149,7 @@ private: (ParamBool) _param_man_arm_gesture, (ParamFloat) _param_man_kill_gest_t, (ParamBool) _param_com_arm_swisbtn, + (ParamInt) _param_rc_map_pay_sw, (ParamInt) _param_fltmode_1, (ParamInt) _param_fltmode_2, (ParamInt) _param_fltmode_3,