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

@@ -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)
 {

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);

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();
 }
 

src/main/io/serial_msp.c

@@ -1688,6 +1688,7 @@ void mspProcess(void)
             while (!isSerialTransmitBufferEmpty(candidatePort->port)) {
                 delay(50);
             }
+            stopMotors();
             systemReset();
         }
     }

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 (!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 gyro was not detected due to whatever reason, we give up now.
-    if (!sensorsOK)
         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);
 }