Skip to content

Commit 9af14c2

Browse files
committed
RC_Channel: require RC switch position transition for arming
currently if you turn your transmitter on at runtime we may try to arm the vehicle if your arming switches are asserted. This patch changes things to we don't trust the first position seen from an RC receiver - we must see a transission for the arming options
1 parent 3514125 commit 9af14c2

File tree

2 files changed

+34
-0
lines changed

2 files changed

+34
-0
lines changed

libraries/RC_Channel/RC_Channel.cpp

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -624,6 +624,7 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos
624624
switch (ch_option) {
625625
// the following functions do not need to be initialised:
626626
case AUX_FUNC::ARMDISARM:
627+
case AUX_FUNC::ARMDISARM_AIRMODE:
627628
case AUX_FUNC::BATTERY_MPPT_ENABLE:
628629
case AUX_FUNC::CAMERA_TRIGGER:
629630
case AUX_FUNC::CLEAR_WP:
@@ -828,6 +829,14 @@ bool RC_Channel::read_aux()
828829
return false;
829830
}
830831

832+
if (!switch_state.initialised) {
833+
switch_state.initialised = true;
834+
if (init_position_on_first_radio_read((AUX_FUNC)option.get())) {
835+
switch_state.current_position = (int8_t)new_position;
836+
switch_state.debounce_position = (int8_t)new_position;
837+
}
838+
}
839+
831840
if (!debounce_completed((int8_t)new_position)) {
832841
return false;
833842
}
@@ -845,6 +854,23 @@ bool RC_Channel::read_aux()
845854
return true;
846855
}
847856

857+
// returns true if the first time we successfully read the
858+
// channel's three-position-switch position we should record that
859+
// position as the current position *without* executing the
860+
// associated auxiliary function. e.g. do not attempt to arm a
861+
// vehicle when the user turns on their transmitter with the arm
862+
// switch high!
863+
bool RC_Channel::init_position_on_first_radio_read(AUX_FUNC func) const
864+
{
865+
switch (func) {
866+
case AUX_FUNC::ARMDISARM_AIRMODE:
867+
case AUX_FUNC::ARMDISARM:
868+
// we do not want to process
869+
return true;
870+
default:
871+
return false;
872+
}
873+
}
848874

849875
void RC_Channel::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
850876
{

libraries/RC_Channel/RC_Channel.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -407,11 +407,19 @@ class RC_Channel {
407407
int8_t debounce_position = -1;
408408
int8_t current_position = -1;
409409
uint32_t last_edge_time_ms;
410+
bool initialised;
410411
} switch_state;
411412

412413
void reset_mode_switch();
413414
void read_mode_switch();
414415
bool debounce_completed(int8_t position);
416+
// returns true if the first time we successfully read the
417+
// channel's three-position-switch position we should record that
418+
// position as the current position *without* executing the
419+
// associated auxiliary function. e.g. do not attempt to arm a
420+
// vehicle when the user turns on their transmitter with the arm
421+
// switch high!
422+
bool init_position_on_first_radio_read(AUX_FUNC func) const;
415423

416424
#if AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED
417425
// Structure to lookup switch change announcements

0 commit comments

Comments
 (0)