From 36156983237ae8af7dce932a343df6eb45558114 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 17 Nov 2024 11:16:39 +0000 Subject: [PATCH 01/14] Plane: `do_aux_function` call `ice_control.do_aux_function` --- ArduPlane/RC_Channel.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 2dc6f1edaab01..637e583856035 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -172,9 +172,6 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, case AUX_FUNC::FW_AUTOTUNE: case AUX_FUNC::VFWD_THR_OVERRIDE: case AUX_FUNC::PRECISION_LOITER: -#if AP_ICENGINE_ENABLED - case AUX_FUNC::ICE_START_STOP: -#endif #if QAUTOTUNE_ENABLED case AUX_FUNC::AUTOTUNE_TEST_GAINS: #endif @@ -194,6 +191,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, #endif case AUX_FUNC::TER_DISABLE: case AUX_FUNC::CROW_SELECT: +#if AP_ICENGINE_ENABLED + case AUX_FUNC::ICE_START_STOP: +#endif run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); break; @@ -296,9 +296,6 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch case AUX_FUNC::FLAP: case AUX_FUNC::FBWA_TAILDRAGGER: case AUX_FUNC::AIRBRAKE: -#if AP_ICENGINE_ENABLED - case AUX_FUNC::ICE_START_STOP: -#endif break; // input labels, nothing to do #if HAL_QUADPLANE_ENABLED @@ -468,6 +465,12 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch break; #endif +#if AP_ICENGINE_ENABLED + case AUX_FUNC::ICE_START_STOP: + plane.g2.ice_control.do_aux_function(ch_flag); + break; +#endif + default: return RC_Channel::do_aux_function(ch_option, ch_flag); } From 2f0a598cee8045840da72d6d443aafcdad2d368a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 17 Nov 2024 11:16:59 +0000 Subject: [PATCH 02/14] AP_ICEngine: fully move to aux function removing dedicated min PWM and high low thresholds --- libraries/AP_ICEngine/AP_ICEngine.cpp | 62 ++++++++------------------- libraries/AP_ICEngine/AP_ICEngine.h | 12 +++--- 2 files changed, 23 insertions(+), 51 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 37c04689fcf0a..b9cfdb1e98d26 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -30,8 +30,6 @@ extern const AP_HAL::HAL& hal; -#define AP_ICENGINE_START_CHAN_DEBOUNCE_MS 300 - const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Param: ENABLE @@ -163,7 +161,8 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Description: This is a minimum PWM value for engine start channel for an engine stop to be commanded. Setting this value will avoid RC input glitches with low PWM values from causing an unwanted engine stop. A value of zero means any PWM above 800 and below 1300 triggers an engine stop. To stop the engine start channel must above the larger of this value and 800 and below 1300. // @User: Standard // @Range: 0 1300 - AP_GROUPINFO("STARTCHN_MIN", 16, AP_ICEngine, start_chan_min_pwm, 0), + + // 16 was STARTCHN_MIN #if AP_RPM_ENABLED // @Param: REDLINE_RPM @@ -282,6 +281,12 @@ void AP_ICEngine::param_conversion() } } +// Handle incoming aux function +void AP_ICEngine::do_aux_function(const RC_Channel::AuxSwitchPos ch_flag) +{ + aux_pos = ch_flag; +} + /* update engine state */ @@ -291,51 +296,20 @@ void AP_ICEngine::update(void) return; } - uint16_t cvalue = 1500; - RC_Channel *c = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ICE_START_STOP); - if (c != nullptr && rc().has_valid_input()) { - // get starter control channel - cvalue = c->get_radio_in(); - - if (cvalue < start_chan_min_pwm) { - cvalue = start_chan_last_value; - } - - // snap the input to either 1000, 1500, or 2000 - // this is useful to compare a debounce changed value - // while ignoring tiny noise - if (cvalue >= RC_Channel::AUX_PWM_TRIGGER_HIGH) { - cvalue = 2000; - } else if ((cvalue > 800) && (cvalue <= RC_Channel::AUX_PWM_TRIGGER_LOW)) { - cvalue = 1300; - } else { - cvalue = 1500; - } - } - bool should_run = false; uint32_t now = AP_HAL::millis(); - // debounce timer to protect from spurious changes on start_chan rc input - // If the cached value is the same, reset timer - if (start_chan_last_value == cvalue) { - start_chan_last_ms = now; - } else if (now - start_chan_last_ms >= AP_ICENGINE_START_CHAN_DEBOUNCE_MS) { - // if it has changed, and stayed changed for the duration, then use that new value - start_chan_last_value = cvalue; - } - - if (state == ICE_START_HEIGHT_DELAY && start_chan_last_value >= RC_Channel::AUX_PWM_TRIGGER_HIGH) { + if ((state == ICE_START_HEIGHT_DELAY) && (aux_pos == RC_Channel::AuxSwitchPos::HIGH)) { // user is overriding the height start delay and asking for // immediate start. Put into ICE_OFF so that the logic below // can start the engine now state = ICE_OFF; } - if (state == ICE_OFF && start_chan_last_value >= RC_Channel::AUX_PWM_TRIGGER_HIGH) { + if ((state == ICE_OFF) && (aux_pos == RC_Channel::AuxSwitchPos::HIGH)) { should_run = true; - } else if (start_chan_last_value <= RC_Channel::AUX_PWM_TRIGGER_LOW) { + } else if (aux_pos == RC_Channel::AuxSwitchPos::LOW) { should_run = false; // clear the single start flag now that we will be stopping the engine @@ -622,15 +596,13 @@ bool AP_ICEngine::engine_control(float start_control, float cold_start, float he GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: already running"); return false; } - RC_Channel *c = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ICE_START_STOP); - if (c != nullptr && rc().has_valid_input()) { - // get starter control channel - uint16_t cvalue = c->get_radio_in(); - if (cvalue >= start_chan_min_pwm && cvalue <= RC_Channel::AUX_PWM_TRIGGER_LOW) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: start control disabled"); - return false; - } + + // get starter control channel + if (aux_pos == RC_Channel::AuxSwitchPos::LOW) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: start control disabled by aux function"); + return false; } + if (height_delay > 0) { height_pending = true; initial_height = 0; diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 4afc96f2ada6d..7409fe6b1aa12 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -27,6 +27,7 @@ #include #include #include +#include #if AP_ICENGINE_TCA9554_STARTER_ENABLED #include "AP_ICEngine_TCA9554.h" @@ -69,6 +70,9 @@ class AP_ICEngine { // do we have throttle while disarmed enabled? bool allow_throttle_while_disarmed(void) const; + // Handle incoming aux function trigger + void do_aux_function(const RC_Channel::AuxSwitchPos ch_flag); + #if AP_RELAY_ENABLED // Needed for param conversion from relay numbers to functions bool get_legacy_ignition_relay_index(int8_t &num); @@ -93,9 +97,6 @@ class AP_ICEngine { // enable library AP_Int8 enable; - // min pwm on start channel for engine stop - AP_Int16 start_chan_min_pwm; - #if AP_RPM_ENABLED // which RPM instance to use AP_Int8 rpm_instance; @@ -169,9 +170,8 @@ class AP_ICEngine { return (options & uint16_t(option)) != 0; } - // start_chan debounce - uint16_t start_chan_last_value = 1500; - uint32_t start_chan_last_ms; + // Last aux function value + RC_Channel::AuxSwitchPos aux_pos = RC_Channel::AuxSwitchPos::MIDDLE; #if AP_ICENGINE_TCA9554_STARTER_ENABLED AP_ICEngine_TCA9554 tca9554_starter; From f3d2d5560ba63b0a79b4d611064336f41eb18c69 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 17 Nov 2024 15:07:06 +0000 Subject: [PATCH 03/14] Tools: autotest: quadplane: MAV_CMD_DO_ENGINE_CONTROL: wait after RC input change before sending command --- Tools/autotest/quadplane.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 721752a5c337d..b099b89df8283 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1234,6 +1234,7 @@ def MAV_CMD_DO_ENGINE_CONTROL(self): self.start_subtest("Check start chan control disable") old_start_channel_value = self.get_rc_channel_value(rc_engine_start_chan) self.set_rc(rc_engine_start_chan, 1000) + self.delay_sim_time(1) # Make sure the RC change has registered self.context_collect('STATUSTEXT') method(mavutil.mavlink.MAV_CMD_DO_ENGINE_CONTROL, p1=1, want_result=mavutil.mavlink.MAV_RESULT_FAILED) self.wait_statustext("start control disabled", check_context=True) From a87e2e6e79e44c74c67d827359371a2fd6c11b4a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:46 +0000 Subject: [PATCH 04/14] AP_Button: move to AuxFuncTrigger structure --- libraries/AP_Button/AP_Button.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Button/AP_Button.cpp b/libraries/AP_Button/AP_Button.cpp index c21a947e22a74..1ce5187c8c268 100644 --- a/libraries/AP_Button/AP_Button.cpp +++ b/libraries/AP_Button/AP_Button.cpp @@ -278,7 +278,7 @@ void AP_Button::run_aux_functions(bool force) GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Button %i: executing (%s %s)", i+1, str, rc_channel->string_for_aux_pos(pos)); } #endif - rc_channel->run_aux_function(func, pos, RC_Channel::AuxFuncTriggerSource::BUTTON); + rc_channel->run_aux_function(func, pos, RC_Channel::AuxFuncTrigger::Source::BUTTON); } } From 1ca559c455f17c51381b485611f24299d5ea3605 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:46 +0000 Subject: [PATCH 05/14] AP_Mission: move to AuxFuncTrigger structure --- libraries/AP_Mission/AP_Mission_Commands.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index ab69c468245a6..c25b34131dd82 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -31,7 +31,7 @@ bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command default: return false; } - rc().run_aux_function(function, pos, RC_Channel::AuxFuncTriggerSource::MISSION); + rc().run_aux_function(function, pos, RC_Channel::AuxFuncTrigger::Source::MISSION); return true; } #endif // AP_RC_CHANNEL_ENABLED From cf83fca94457ddbcf9d1229a8415161b17a8cbbf Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:46 +0000 Subject: [PATCH 06/14] AP_Scripting: move to AuxFuncTrigger structure --- libraries/AP_Scripting/generator/description/bindings.desc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index d0ff5b1e490b5..908fd7f55282e 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -398,7 +398,7 @@ singleton RC_Channels rename rc singleton RC_Channels scheduler-semaphore singleton RC_Channels method get_pwm boolean uint8_t 1 NUM_RC_CHANNELS uint16_t'Null singleton RC_Channels method find_channel_for_option RC_Channel RC_Channel::AUX_FUNC'enum 0 UINT16_MAX -singleton RC_Channels method run_aux_function boolean RC_Channel::AUX_FUNC'enum 0 UINT16_MAX RC_Channel::AuxSwitchPos'enum RC_Channel::AuxSwitchPos::LOW RC_Channel::AuxSwitchPos::HIGH RC_Channel::AuxFuncTriggerSource::SCRIPTING'literal +singleton RC_Channels method run_aux_function boolean RC_Channel::AUX_FUNC'enum 0 UINT16_MAX RC_Channel::AuxSwitchPos'enum RC_Channel::AuxSwitchPos::LOW RC_Channel::AuxSwitchPos::HIGH RC_Channel::AuxFuncTrigger::Source::SCRIPTING'literal singleton RC_Channels method has_valid_input boolean singleton RC_Channels method lua_rc_channel RC_Channel uint8_t 1 NUM_RC_CHANNELS singleton RC_Channels method lua_rc_channel rename get_channel From 98efead1be1e6ed4f64ea76799f3021b79ac0d93 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:46 +0000 Subject: [PATCH 07/14] GCS_MAVLink: move to AuxFuncTrigger structure --- libraries/GCS_MAVLink/GCS_Common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e670339e7b756..6409b172f9d3b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3110,7 +3110,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_int } const RC_Channel::AUX_FUNC aux_func = (RC_Channel::AUX_FUNC)packet.param1; const RC_Channel::AuxSwitchPos position = (RC_Channel::AuxSwitchPos)packet.param2; - if (!rc().run_aux_function(aux_func, position, RC_Channel::AuxFuncTriggerSource::MAVLINK)) { + if (!rc().run_aux_function(aux_func, position, RC_Channel::AuxFuncTrigger::Source::MAVLINK)) { // note that this is not quite right; we could be more nuanced // about our return code here. return MAV_RESULT_FAILED; From 1f1ed392281edbeff00ceda276cf96bed636dba5 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:46 +0000 Subject: [PATCH 08/14] RC_Channel: move to AuxFuncTrigger structure --- libraries/RC_Channel/RC_Channel.cpp | 23 +++++++++++++++++------ libraries/RC_Channel/RC_Channel.h | 27 +++++++++++++++++---------- 2 files changed, 34 insertions(+), 16 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index a30c16f4c9931..cf23accdb93dc 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -771,7 +771,7 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos #endif #if AP_AHRS_ENABLED case AUX_FUNC::AHRS_TYPE: - run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); + run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT); break; #endif default: @@ -969,7 +969,7 @@ bool RC_Channel::read_aux() #endif // debounced; undertake the action: - run_aux_function(_option, new_position, AuxFuncTriggerSource::RC); + run_aux_function(_option, new_position, AuxFuncTrigger::Source::RC, get_radio_in()); return true; } @@ -1394,12 +1394,20 @@ void RC_Channel::do_aux_function_retract_mount(const AuxSwitchPos ch_flag, const } #endif // HAL_MOUNT_ENABLED -bool RC_Channel::run_aux_function(AUX_FUNC ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source) +bool RC_Channel::run_aux_function(AUX_FUNC ch_option, AuxSwitchPos pos, AuxFuncTrigger::Source source, int16_t pwm) { #if AP_SCRIPTING_ENABLED rc().set_aux_cached(ch_option, pos); #endif - const bool ret = do_aux_function(ch_option, pos); + + const AuxFuncTrigger trigger { + func: ch_option, + pos: pos, + source: source, + pwm: pwm, + }; + + const bool ret = do_aux_function(trigger); #if HAL_LOGGING_ENABLED // @LoggerMessage: AUXF @@ -1410,7 +1418,7 @@ bool RC_Channel::run_aux_function(AUX_FUNC ch_option, AuxSwitchPos pos, AuxFuncT // @Field: pos: switch position when function triggered // @FieldValueEnum: pos: RC_Channel::AuxSwitchPos // @Field: source: source of auxiliary function invocation - // @FieldValueEnum: source: RC_Channel::AuxFuncTriggerSource + // @FieldValueEnum: source: RC_Channel::AuxFuncTrigger::Source // @Field: result: true if function was successful AP::logger().Write( "AUXF", @@ -1429,8 +1437,11 @@ bool RC_Channel::run_aux_function(AUX_FUNC ch_option, AuxSwitchPos pos, AuxFuncT return ret; } -bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag) +bool RC_Channel::do_aux_function(const AuxFuncTrigger &trigger) { + const AUX_FUNC &ch_option = trigger.func; + const AuxSwitchPos &ch_flag = trigger.pos; + switch (ch_option) { #if AP_FENCE_ENABLED case AUX_FUNC::FENCE: diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 7194fa092b80e..6bef8d415ccc1 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -306,19 +306,26 @@ class RC_Channel { HIGH // indicates auxiliary switch is in the high position (pwm >1800) }; - enum class AuxFuncTriggerSource : uint8_t { - INIT, - RC, - BUTTON, - MAVLINK, - MISSION, - SCRIPTING, + // Trigger structure containing the function, position and pwm (if applicable) + struct AuxFuncTrigger { + AUX_FUNC func; + AuxSwitchPos pos; + enum class Source : uint8_t { + INIT, + RC, + BUTTON, + MAVLINK, + MISSION, + SCRIPTING, + } source; + // Trigger PWM, only valid for RC source + int16_t pwm; }; AuxSwitchPos get_aux_switch_pos() const; // wrapper function around do_aux_function which allows us to log - bool run_aux_function(AUX_FUNC ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source); + bool run_aux_function(AUX_FUNC ch_option, AuxSwitchPos pos, AuxFuncTrigger::Source source, int16_t pwm = 0); #if AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED const char *string_for_aux_function(AUX_FUNC function) const; @@ -349,7 +356,7 @@ class RC_Channel { virtual void init_aux_function(AUX_FUNC ch_option, AuxSwitchPos); // virtual function to be overridden my subclasses - virtual bool do_aux_function(AUX_FUNC ch_option, AuxSwitchPos); + virtual bool do_aux_function(const AuxFuncTrigger &trigger); void do_aux_function_armdisarm(const AuxSwitchPos ch_flag); void do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag); @@ -589,7 +596,7 @@ class RC_Channels { // method for other parts of the system (e.g. Button and mavlink) // to trigger auxiliary functions - bool run_aux_function(RC_Channel::AUX_FUNC ch_option, RC_Channel::AuxSwitchPos pos, RC_Channel::AuxFuncTriggerSource source) { + bool run_aux_function(RC_Channel::AUX_FUNC ch_option, RC_Channel::AuxSwitchPos pos, RC_Channel::AuxFuncTrigger::Source source) { return rc_channel(0)->run_aux_function(ch_option, pos, source); } From 3889d3117b398fdda8c9ac11e3593297b89b37b4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:46 +0000 Subject: [PATCH 09/14] ArduCopter: move to AuxFuncTrigger structure --- ArduCopter/RC_Channel.cpp | 9 ++++++--- ArduCopter/RC_Channel.h | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index fdb175f80c5b8..a9d33c414c879 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -140,7 +140,7 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi case AUX_FUNC::CUSTOM_CONTROLLER: case AUX_FUNC::WEATHER_VANE_ENABLE: case AUX_FUNC::TRANSMITTER_TUNING: - run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); + run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT); break; default: RC_Channel::init_aux_function(ch_option, ch_flag); @@ -169,8 +169,11 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode, } // do_aux_function - implement the function invoked by auxiliary switches -bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag) +bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger) { + const AUX_FUNC &ch_option = trigger.func; + const AuxSwitchPos &ch_flag = trigger.pos; + switch(ch_option) { case AUX_FUNC::FLIP: // flip if switch is on, positive throttle and we're actually flying @@ -668,7 +671,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; default: - return RC_Channel::do_aux_function(ch_option, ch_flag); + return RC_Channel::do_aux_function(trigger); } return true; } diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h index b0ba3ebb1492f..07bf285ab59a6 100644 --- a/ArduCopter/RC_Channel.h +++ b/ArduCopter/RC_Channel.h @@ -12,7 +12,7 @@ class RC_Channel_Copter : public RC_Channel protected: void init_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override; - bool do_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override; + bool do_aux_function(const AuxFuncTrigger &trigger) override; private: From 24b6bae9fe1d60d4cfcff3463a1f4529be37de98 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:46 +0000 Subject: [PATCH 10/14] ArduPlane: move to AuxFuncTrigger structure --- ArduPlane/RC_Channel.cpp | 9 ++++++--- ArduPlane/RC_Channel.h | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 637e583856035..e71f53cfa01c5 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -194,7 +194,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, #if AP_ICENGINE_ENABLED case AUX_FUNC::ICE_START_STOP: #endif - run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); + run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT); break; case AUX_FUNC::REVERSE_THROTTLE: @@ -216,8 +216,11 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option, } // do_aux_function - implement the function invoked by auxiliary switches -bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag) +bool RC_Channel_Plane::do_aux_function(const AuxFuncTrigger &trigger) { + const AUX_FUNC &ch_option = trigger.func; + const AuxSwitchPos &ch_flag = trigger.pos; + switch(ch_option) { case AUX_FUNC::INVERTED: plane.inverted_flight = (ch_flag == AuxSwitchPos::HIGH); @@ -472,7 +475,7 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch #endif default: - return RC_Channel::do_aux_function(ch_option, ch_flag); + return RC_Channel::do_aux_function(trigger); } return true; diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index e9b4804aac545..20b03227a2145 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -11,7 +11,7 @@ class RC_Channel_Plane : public RC_Channel void init_aux_function(AUX_FUNC ch_option, AuxSwitchPos ch_flag) override; - bool do_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override; + bool do_aux_function(const AuxFuncTrigger &trigger) override; // called when the mode switch changes position: void mode_switch_changed(modeswitch_pos_t new_pos) override; From 48e1ed07f7445552319420b66b97554992e45556 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:47 +0000 Subject: [PATCH 11/14] Blimp: move to AuxFuncTrigger structure --- Blimp/RC_Channel.cpp | 7 +++++-- Blimp/RC_Channel.h | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/Blimp/RC_Channel.cpp b/Blimp/RC_Channel.cpp index 1132d82330b14..6dafa3b8e55f2 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -99,8 +99,11 @@ void RC_Channel_Blimp::do_aux_function_change_mode(const Mode::Number mode, } // do_aux_function - implement the function invoked by auxiliary switches -bool RC_Channel_Blimp::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag) +bool RC_Channel_Blimp::do_aux_function(const AuxFuncTrigger &trigger) { + const AUX_FUNC &ch_option = trigger.func; + const AuxSwitchPos &ch_flag = trigger.pos; + switch (ch_option) { case AUX_FUNC::SAVE_TRIM: @@ -120,7 +123,7 @@ bool RC_Channel_Blimp::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch break; default: - return RC_Channel::do_aux_function(ch_option, ch_flag); + return RC_Channel::do_aux_function(trigger); } return true; } diff --git a/Blimp/RC_Channel.h b/Blimp/RC_Channel.h index 4a6e5e0156da1..12f9ae2f7ced5 100644 --- a/Blimp/RC_Channel.h +++ b/Blimp/RC_Channel.h @@ -12,7 +12,7 @@ class RC_Channel_Blimp : public RC_Channel protected: void init_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override; - bool do_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override; + bool do_aux_function(const AuxFuncTrigger &trigger) override; private: From a7239d15e7b86e8542aed167a002e1165ef5fc06 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:15:47 +0000 Subject: [PATCH 12/14] Rover: move to AuxFuncTrigger structure --- Rover/RC_Channel.cpp | 7 +++++-- Rover/RC_Channel.h | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/Rover/RC_Channel.cpp b/Rover/RC_Channel.cpp index 75cd088a757a2..3bb7db03858d6 100644 --- a/Rover/RC_Channel.cpp +++ b/Rover/RC_Channel.cpp @@ -130,8 +130,11 @@ void RC_Channel_Rover::do_aux_function_sailboat_motor_3pos(const AuxSwitchPos ch } } -bool RC_Channel_Rover::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag) +bool RC_Channel_Rover::do_aux_function(const AuxFuncTrigger &trigger) { + const AUX_FUNC &ch_option = trigger.func; + const AuxSwitchPos &ch_flag = trigger.pos; + switch (ch_option) { case AUX_FUNC::DO_NOTHING: break; @@ -261,7 +264,7 @@ bool RC_Channel_Rover::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch break; default: - return RC_Channel::do_aux_function(ch_option, ch_flag); + return RC_Channel::do_aux_function(trigger); } diff --git a/Rover/RC_Channel.h b/Rover/RC_Channel.h index 01c83da59b738..ae4c4b60c8643 100644 --- a/Rover/RC_Channel.h +++ b/Rover/RC_Channel.h @@ -12,7 +12,7 @@ class RC_Channel_Rover : public RC_Channel protected: void init_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override; - bool do_aux_function(AUX_FUNC ch_option, AuxSwitchPos) override; + bool do_aux_function(const AuxFuncTrigger &trigger) override; // called when the mode switch changes position: void mode_switch_changed(modeswitch_pos_t new_pos) override; From f2ce8c9b92bef159ed497cd1d00b83c18c05ec5f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:24:35 +0000 Subject: [PATCH 13/14] Plane: RC_Channel: pass trigger object onto ICE --- ArduPlane/RC_Channel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index e71f53cfa01c5..c407062f11512 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -470,7 +470,7 @@ bool RC_Channel_Plane::do_aux_function(const AuxFuncTrigger &trigger) #if AP_ICENGINE_ENABLED case AUX_FUNC::ICE_START_STOP: - plane.g2.ice_control.do_aux_function(ch_flag); + plane.g2.ice_control.do_aux_function(trigger); break; #endif From 957bd4afda2d0443565ad620f9bae769c5e89835 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 13 Dec 2024 21:25:34 +0000 Subject: [PATCH 14/14] AP_ICEngine: reinstate `STARTCHN_MIN` and use trigger structure PWM feild for aux funciton triggers --- libraries/AP_ICEngine/AP_ICEngine.cpp | 12 ++++++++---- libraries/AP_ICEngine/AP_ICEngine.h | 5 ++++- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index b9cfdb1e98d26..5284d2c593f2e 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -161,8 +161,7 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { // @Description: This is a minimum PWM value for engine start channel for an engine stop to be commanded. Setting this value will avoid RC input glitches with low PWM values from causing an unwanted engine stop. A value of zero means any PWM above 800 and below 1300 triggers an engine stop. To stop the engine start channel must above the larger of this value and 800 and below 1300. // @User: Standard // @Range: 0 1300 - - // 16 was STARTCHN_MIN + AP_GROUPINFO("STARTCHN_MIN", 16, AP_ICEngine, start_chan_min_pwm, 0), #if AP_RPM_ENABLED // @Param: REDLINE_RPM @@ -282,9 +281,14 @@ void AP_ICEngine::param_conversion() } // Handle incoming aux function -void AP_ICEngine::do_aux_function(const RC_Channel::AuxSwitchPos ch_flag) +void AP_ICEngine::do_aux_function(const RC_Channel::AuxFuncTrigger &trigger) { - aux_pos = ch_flag; + // If triggered from RC apply start chan min + if ((trigger.source == RC_Channel::AuxFuncTrigger::Source::RC) && (trigger.pwm < start_chan_min_pwm)) { + return; + } + + aux_pos = trigger.pos; } /* diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 7409fe6b1aa12..58cd11bd0ec5f 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -71,7 +71,7 @@ class AP_ICEngine { bool allow_throttle_while_disarmed(void) const; // Handle incoming aux function trigger - void do_aux_function(const RC_Channel::AuxSwitchPos ch_flag); + void do_aux_function(const RC_Channel::AuxFuncTrigger &trigger); #if AP_RELAY_ENABLED // Needed for param conversion from relay numbers to functions @@ -97,6 +97,9 @@ class AP_ICEngine { // enable library AP_Int8 enable; + // min pwm on start channel for engine stop + AP_Int16 start_chan_min_pwm; + #if AP_RPM_ENABLED // which RPM instance to use AP_Int8 rpm_instance;