From 9b4ccdc73a7b3cd0328431ab8575d210a76c1cfc Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 24 Aug 2014 22:59:27 +0100 Subject: [PATCH] Only set OK-to-arm flag when arm switches are disabled. This prevents arming after connecting a battery when the throttle is in low; when using a switch to arm and when the switch is left in the ON position. The previous solution was flawed because of the way rc input is handled. This also removes an extra test of rcOptions by re-arranging the logic that handles arming/disarming via a switch. --- src/main/config/runtime_config.h | 3 +-- src/main/io/rc_controls.c | 18 ++++++++---------- src/main/mw.c | 12 +++++++----- 3 files changed, 16 insertions(+), 17 deletions(-) diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index ed32432e1..2a96c5d5c 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -48,8 +48,7 @@ extern uint8_t rcOptions[CHECKBOX_ITEM_COUNT]; typedef enum { OK_TO_ARM = (1 << 0), PREVENT_ARMING = (1 << 1), - SEEN_BOXARM_OFF = (1 << 2), - ARMED = (1 << 3) + ARMED = (1 << 2) } armingFlag_e; extern uint8_t armingFlags; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 0e882e0e0..c79067aa8 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -41,7 +41,6 @@ #include "rx/rx.h" #include "io/rc_controls.h" - int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW bool areSticksInApModePosition(uint16_t ap_mode) @@ -85,16 +84,15 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat // perform actions if (activate[BOXARM] > 0) { - // Arming via ARM BOX - if (throttleStatus == THROTTLE_LOW) { - if (rcOptions[BOXARM] && ARMING_FLAG(OK_TO_ARM)) { - mwArm(); + if (rcOptions[BOXARM]) { + // Arming via ARM BOX + if (throttleStatus == THROTTLE_LOW) { + if (ARMING_FLAG(OK_TO_ARM)) { + mwArm(); + } } - } - - // Disarming via ARM BOX - if (!rcOptions[BOXARM]) { - ENABLE_ARMING_FLAG(SEEN_BOXARM_OFF); + } else { + // Disarming via ARM BOX if (ARMING_FLAG(ARMED)) { mwDisarm(); } diff --git a/src/main/mw.c b/src/main/mw.c index c42aabdce..e79d3e6dd 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -244,7 +244,9 @@ void annexCode(void) if (ARMING_FLAG(ARMED)) { LED0_ON; } else { - ENABLE_ARMING_FLAG(OK_TO_ARM); + if (rcOptions[BOXARM] == 0) { + ENABLE_ARMING_FLAG(OK_TO_ARM); + } if (isCalibrating()) { LED0_TOGGLE; @@ -255,10 +257,6 @@ void annexCode(void) DISABLE_ARMING_FLAG(OK_TO_ARM); } - if (rcOptions[BOXARM] && !ARMING_FLAG(SEEN_BOXARM_OFF)) { - DISABLE_ARMING_FLAG(OK_TO_ARM); - } - if (rcOptions[BOXAUTOTUNE]) { DISABLE_ARMING_FLAG(OK_TO_ARM); } @@ -272,6 +270,10 @@ void annexCode(void) updateWarningLed(currentTime); } +#if 0 + debug[0] = armingFlags; +#endif + #ifdef TELEMETRY checkTelemetryState(); #endif