From c1fcdabdc1e83a36c6c3f30594e6aa1c24d401e1 Mon Sep 17 00:00:00 2001
From: dongie <timecop@gmail.com>
Date: Fri, 2 May 2014 18:27:23 +0900
Subject: [PATCH] rearranged startup / sensor detection for production testing.
 moved dead gyro endless loop out of the way.

---
 src/main.c    | 38 ++++++++++++++++++++++----------------
 src/mw.h      |  2 +-
 src/sensors.c |  9 ++++++---
 3 files changed, 29 insertions(+), 20 deletions(-)

diff --git a/src/main.c b/src/main.c
index c66a1d6476..31e7274972 100755
--- a/src/main.c
+++ b/src/main.c
@@ -32,6 +32,7 @@ int main(void)
     uint8_t i;
     drv_pwm_config_t pwm_params;
     drv_adc_config_t adc_params;
+    bool sensorsOK = false;
 #ifdef SOFTSERIAL_LOOPBACK
     serialPort_t* loopbackPort1 = NULL;
     serialPort_t* loopbackPort2 = NULL;
@@ -63,15 +64,33 @@ int main(void)
     // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
     sensorsSet(SENSORS_SET);
     // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
-    sensorsAutodetect();
-    imuInit(); // Mag is initialized inside imuInit
-    mixerInit(); // this will set core.useServo var depending on mixer type
+    sensorsOK = sensorsAutodetect();
 
     // production debug output
 #ifdef PROD_DEBUG
     productionDebug();
 #endif
 
+    // if gyro was not detected due to whatever reason, we give up now.
+    if (!sensorsOK)
+        failureMode(3);
+
+    LED1_ON;
+    LED0_OFF;
+    for (i = 0; i < 10; i++) {
+        LED1_TOGGLE;
+        LED0_TOGGLE;
+        delay(25);
+        BEEP_ON;
+        delay(25);
+        BEEP_OFF;
+    }
+    LED0_OFF;
+    LED1_OFF;
+
+    imuInit(); // Mag is initialized inside imuInit
+    mixerInit(); // this will set core.useServo var depending on mixer type
+
     serialInit(mcfg.serial_baudrate);
 
     // when using airplane/wing mixer, servo/motor outputs are remapped
@@ -169,19 +188,6 @@ int main(void)
     calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
     f.SMALL_ANGLE = 1;
 
-    LED1_ON;
-    LED0_OFF;
-    for (i = 0; i < 10; i++) {
-        LED1_TOGGLE;
-        LED0_TOGGLE;
-        delay(25);
-        BEEP_ON;
-        delay(25);
-        BEEP_OFF;
-    }
-    LED0_OFF;
-    LED1_OFF;
-
     // loopy
     while (1) {
         loop();
diff --git a/src/mw.h b/src/mw.h
index 59877b14ee..c368cdc5f3 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -412,7 +412,7 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat);
 int getEstimatedAltitude(void);
 
 // Sensors
-void sensorsAutodetect(void);
+bool sensorsAutodetect(void);
 void batteryInit(void);
 uint16_t batteryAdcToVoltage(uint16_t src);
 void ACC_getADC(void);
diff --git a/src/sensors.c b/src/sensors.c
index ad044f7d3b..4502382f8d 100755
--- a/src/sensors.c
+++ b/src/sensors.c
@@ -22,13 +22,14 @@ uint8_t accHardware = ACC_DEFAULT;  // which accel chip is used/detected
 
 #ifdef FY90Q
 // FY90Q analog gyro/acc
-void sensorsAutodetect(void)
+bool sensorsAutodetect(void)
 {
     adcSensorInit(&acc, &gyro);
+    return true;
 }
 #else
 // AfroFlight32 i2c sensors
-void sensorsAutodetect(void)
+bool sensorsAutodetect(void)
 {
     int16_t deg, min;
     drv_adxl345_config_t acc_params;
@@ -43,7 +44,7 @@ void sensorsAutodetect(void)
         ;
     } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
         // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
-        failureMode(3);
+        return false;
     }
 
     // Accelerometer. Fuck it. Let user break shit.
@@ -127,6 +128,8 @@ retry:
         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
     else
         magneticDeclination = 0.0f;
+
+    return true;
 }
 #endif