From b9e1283809ec97353722f7b85ce7e5481e035259 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 23 Feb 2015 13:59:41 +0000
Subject: [PATCH] Ensure motors are stopped before a reboot.  Ensure hard fault
 handler doesn't use potentially unitialised data to update the motors.  Pause
 for 50ms before rebooting after updating disabling the motors to ensure the
 timer hardware and ESCs havea chance to react.

This commit might help with #123
---
 src/main/flight/mixer.c  |  7 +++++++
 src/main/flight/mixer.h  |  1 +
 src/main/io/serial_cli.c |  1 +
 src/main/io/serial_msp.c |  1 +
 src/main/main.c          | 33 +++++++++++++++++++++++++++------
 5 files changed, 37 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index d247b5bed6..31af981d38 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -487,6 +487,13 @@ void writeAllMotors(int16_t mc)
     writeMotors();
 }
 
+void stopMotors(void)
+{
+    writeAllMotors(escAndServoConfig->mincommand);
+
+    delay(50); // give the timers and ESCs a chance to react.
+}
+
 #ifndef USE_QUAD_MIXER_ONLY
 static void airplaneMixer(void)
 {
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 38bba6b741..627199af3b 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -109,3 +109,4 @@ void mixerLoadMix(int index, motorMixer_t *customMixers);
 void mixerResetMotors(void);
 void mixTable(void);
 void writeMotors(void);
+void stopMotors(void);
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 29c2f5497c..de9bf0d84a 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -1344,6 +1344,7 @@ static void cliRateProfile(char *cmdline)
 static void cliReboot(void) {
     cliPrint("\r\nRebooting");
     waitForSerialPortToFinishTransmitting(cliPort);
+    stopMotors();
     systemReset();
 }
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 942c009a80..408bea6539 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -1688,6 +1688,7 @@ void mspProcess(void)
             while (!isSerialTransmitBufferEmpty(candidatePort->port)) {
                 delay(50);
             }
+            stopMotors();
             systemReset();
         }
     }
diff --git a/src/main/main.c b/src/main/main.c
index 7e530b3f32..f9a5c4ab38 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -127,11 +127,20 @@ void SetSysClock(void);
 void SetSysClock(bool overclock);
 #endif
 
+typedef enum {
+    SYSTEM_STATE_INITIALISING   = 0,
+    SYSTEM_STATE_CONFIG_LOADED  = (1 << 0),
+    SYSTEM_STATE_SENSORS_READY  = (1 << 1),
+    SYSTEM_STATE_MOTORS_READY   = (1 << 2),
+    SYSTEM_STATE_READY          = (1 << 7)
+} systemState_e;
+
+static uint8_t systemState = SYSTEM_STATE_INITIALISING;
+
 void init(void)
 {
     uint8_t i;
     drv_pwm_config_t pwm_params;
-    bool sensorsOK = false;
 
     printfSupportInit();
 
@@ -140,6 +149,8 @@ void init(void)
     ensureEEPROMContainsValidData();
     readEEPROM();
 
+    systemState |= SYSTEM_STATE_CONFIG_LOADED;
+
 #ifdef STM32F303
     // start fpu
     SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
@@ -222,6 +233,8 @@ void init(void)
 
     mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
 
+    systemState |= SYSTEM_STATE_MOTORS_READY;
+
 #ifdef BEEPER
     beeperConfig_t beeperConfig = {
         .gpioPin = BEEP_PIN,
@@ -300,11 +313,12 @@ void init(void)
     }
 #endif
 
-    sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
-
-    // if gyro was not detected due to whatever reason, we give up now.
-    if (!sensorsOK)
+    if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) {
+        // if gyro was not detected due to whatever reason, we give up now.
         failureMode(3);
+    }
+
+    systemState |= SYSTEM_STATE_SENSORS_READY;
 
     LED1_ON;
     LED0_OFF;
@@ -329,7 +343,9 @@ void init(void)
     serialInit(&masterConfig.serialConfig);
 
     failsafe = failsafeInit(&masterConfig.rxConfig);
+
     beepcodeInit(failsafe);
+
     rxInit(&masterConfig.rxConfig, failsafe);
 
 #ifdef GPS
@@ -425,6 +441,8 @@ void init(void)
 #ifdef CJMCU
     LED2_ON;
 #endif
+
+    systemState |= SYSTEM_STATE_READY;
 }
 
 #ifdef SOFTSERIAL_LOOPBACK
@@ -453,6 +471,9 @@ int main(void) {
 void HardFault_Handler(void)
 {
     // fall out of the sky
-    writeAllMotors(masterConfig.escAndServoConfig.mincommand);
+    uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
+    if ((systemState & requiredState) == requiredState) {
+        stopMotors();
+    }
     while (1);
 }