From c1fcdabdc1e83a36c6c3f30594e6aa1c24d401e1 Mon Sep 17 00:00:00 2001 From: dongie 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