diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 8982fff788..d5e11bf077 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -48,6 +48,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -1453,7 +1454,7 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) blackboxWrite(0); break; case FLIGHT_LOG_EVENT_LOG_END: - blackboxPrint("End of log"); + blackboxPrintf("End of log (disarm reason:%d)", getDisarmReason()); blackboxWrite(0); break; } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a725b084d1..d34fdaa6ba 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -45,6 +45,7 @@ #include "sensors/gyro.h" #include "sensors/battery.h" +#include "fc/fc_core.h" #include "fc/cli.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -103,6 +104,7 @@ int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero static bool isRXDataNew; +static disarmReason_t lastDisarmReason = DISARM_NONE; bool isCalibrating(void) { @@ -222,9 +224,10 @@ void annexCode(void) } } -void mwDisarm(void) +void mwDisarm(disarmReason_t disarmReason) { if (ARMING_FLAG(ARMED)) { + lastDisarmReason = disarmReason; DISABLE_ARMING_FLAG(ARMED); #ifdef BLACKBOX @@ -237,6 +240,11 @@ void mwDisarm(void) } } +disarmReason_t getDisarmReason(void) +{ + return lastDisarmReason; +} + #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) void releaseSharedTelemetryPorts(void) { @@ -302,7 +310,7 @@ void processRx(timeUs_t currentTimeUs) // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) - mwDisarm(); + mwDisarm(DISARM_SWITCH_3D); } updateRSSI(currentTimeUs); @@ -329,7 +337,7 @@ void processRx(timeUs_t currentTimeUs) && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over - mwDisarm(); + mwDisarm(DISARM_TIMEOUT); armedBeeperOn = false; } else { // still armed; do warning beeps while armed diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 420b5fae71..da2a027e14 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -19,9 +19,21 @@ extern int16_t telemTemperature1; +typedef enum disarmReason_e { + DISARM_NONE = 0, + DISARM_TIMEOUT = 1, + DISARM_STICKS = 2, + DISARM_SWITCH_3D = 3, + DISARM_SWITCH = 4, + DISARM_KILLSWITCH = 5, + DISARM_FAILSAFE = 6, + DISARM_NAVIGATION = 7 +} disarmReason_t; + void handleInflightCalibrationStickPosition(); -void mwDisarm(void); +void mwDisarm(disarmReason_t disarmReason); void mwArm(void); +disarmReason_t getDisarmReason(void); bool isCalibrating(void); \ No newline at end of file diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 4643dc3936..23beca60c0 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -179,9 +179,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s rcDisarmTicks++; if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX) if (disarm_kill_switch) { - mwDisarm(); + mwDisarm(DISARM_SWITCH); } else if (throttleStatus == THROTTLE_LOW) { - mwDisarm(); + mwDisarm(DISARM_SWITCH); } } } @@ -193,7 +193,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s // KILLSWITCH disarms instantly if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) { - mwDisarm(); + mwDisarm(DISARM_KILLSWITCH); } if (rcDelayCommand != 20) { @@ -208,7 +208,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s return; } else if (ARMING_FLAG(ARMED)) { - mwDisarm(); + mwDisarm(DISARM_STICKS); } else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 04c085627c..4e6daaf48e 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -111,5 +111,3 @@ bool sensors(uint32_t mask); void sensorsSet(uint32_t mask); void sensorsClear(uint32_t mask); uint32_t sensorsMask(void); - -void mwDisarm(void); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2c9e5a095d..94d88d603c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -34,6 +34,7 @@ #include "io/beeper.h" +#include "fc/fc_core.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -143,6 +144,11 @@ void failsafeReset(void) failsafeState.receivingRxDataPeriodPreset = 0; failsafeState.phase = FAILSAFE_IDLE; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; + + failsafeState.lastGoodRcCommand[ROLL] = 0; + failsafeState.lastGoodRcCommand[PITCH] = 0; + failsafeState.lastGoodRcCommand[YAW] = 0; + failsafeState.lastGoodRcCommand[THROTTLE] = 1000; } void failsafeInit(void) @@ -461,7 +467,7 @@ void failsafeUpdateState(void) case FAILSAFE_LANDED: ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link - mwDisarm(); + mwDisarm(DISARM_FAILSAFE); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; failsafeState.controlling = false; // Failsafe no longer in control of the machine - release control to pilot diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4816b04335..4879be504b 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -34,6 +34,7 @@ #include "drivers/time.h" +#include "fc/fc_core.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -1015,7 +1016,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio UNUSED(previousState); if (navConfig()->general.flags.disarm_on_landing) { - mwDisarm(); + mwDisarm(DISARM_NAVIGATION); } return NAV_FSM_EVENT_SUCCESS; diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 998b64358a..c5f58a773e 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -50,6 +50,7 @@ #include "io/serial.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -259,7 +260,7 @@ void ltm_xframe(sbuf_t *dst) #endif ltm_serialise_8(dst, sensorStatus); ltm_serialise_8(dst, ltm_x_counter); - ltm_serialise_8(dst, 0); + ltm_serialise_8(dst, getDisarmReason()); ltm_serialise_8(dst, 0); ltm_x_counter++; // overflow is OK }