mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-18 05:45:31 +03:00
rearranged startup / sensor detection for production testing. moved dead gyro endless loop out of the way.
This commit is contained in:
parent
2d248676f5
commit
c1fcdabdc1
3 changed files with 29 additions and 20 deletions
38
src/main.c
38
src/main.c
|
@ -32,6 +32,7 @@ int main(void)
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
drv_pwm_config_t pwm_params;
|
drv_pwm_config_t pwm_params;
|
||||||
drv_adc_config_t adc_params;
|
drv_adc_config_t adc_params;
|
||||||
|
bool sensorsOK = false;
|
||||||
#ifdef SOFTSERIAL_LOOPBACK
|
#ifdef SOFTSERIAL_LOOPBACK
|
||||||
serialPort_t* loopbackPort1 = NULL;
|
serialPort_t* loopbackPort1 = NULL;
|
||||||
serialPort_t* loopbackPort2 = 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
|
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
|
||||||
sensorsSet(SENSORS_SET);
|
sensorsSet(SENSORS_SET);
|
||||||
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
|
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
|
||||||
sensorsAutodetect();
|
sensorsOK = sensorsAutodetect();
|
||||||
imuInit(); // Mag is initialized inside imuInit
|
|
||||||
mixerInit(); // this will set core.useServo var depending on mixer type
|
|
||||||
|
|
||||||
// production debug output
|
// production debug output
|
||||||
#ifdef PROD_DEBUG
|
#ifdef PROD_DEBUG
|
||||||
productionDebug();
|
productionDebug();
|
||||||
#endif
|
#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);
|
serialInit(mcfg.serial_baudrate);
|
||||||
|
|
||||||
// when using airplane/wing mixer, servo/motor outputs are remapped
|
// 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
|
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
|
||||||
f.SMALL_ANGLE = 1;
|
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
|
// loopy
|
||||||
while (1) {
|
while (1) {
|
||||||
loop();
|
loop();
|
||||||
|
|
2
src/mw.h
2
src/mw.h
|
@ -412,7 +412,7 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat);
|
||||||
int getEstimatedAltitude(void);
|
int getEstimatedAltitude(void);
|
||||||
|
|
||||||
// Sensors
|
// Sensors
|
||||||
void sensorsAutodetect(void);
|
bool sensorsAutodetect(void);
|
||||||
void batteryInit(void);
|
void batteryInit(void);
|
||||||
uint16_t batteryAdcToVoltage(uint16_t src);
|
uint16_t batteryAdcToVoltage(uint16_t src);
|
||||||
void ACC_getADC(void);
|
void ACC_getADC(void);
|
||||||
|
|
|
@ -22,13 +22,14 @@ uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
|
||||||
|
|
||||||
#ifdef FY90Q
|
#ifdef FY90Q
|
||||||
// FY90Q analog gyro/acc
|
// FY90Q analog gyro/acc
|
||||||
void sensorsAutodetect(void)
|
bool sensorsAutodetect(void)
|
||||||
{
|
{
|
||||||
adcSensorInit(&acc, &gyro);
|
adcSensorInit(&acc, &gyro);
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
// AfroFlight32 i2c sensors
|
// AfroFlight32 i2c sensors
|
||||||
void sensorsAutodetect(void)
|
bool sensorsAutodetect(void)
|
||||||
{
|
{
|
||||||
int16_t deg, min;
|
int16_t deg, min;
|
||||||
drv_adxl345_config_t acc_params;
|
drv_adxl345_config_t acc_params;
|
||||||
|
@ -43,7 +44,7 @@ void sensorsAutodetect(void)
|
||||||
;
|
;
|
||||||
} else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
|
} else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
|
||||||
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
|
// 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.
|
// 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
|
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
||||||
else
|
else
|
||||||
magneticDeclination = 0.0f;
|
magneticDeclination = 0.0f;
|
||||||
|
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue