Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +03:00
Code Issues Wiki Activity

Fixed unnecessarily long startup time due to gyro detection.

Browse source
This commit is contained in:
mikeller 2020-04-07 01:03:51 +12:00
parent 00f2e2a772
commit 489156602f
5 changed files with 51 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

41
src/main/sensors/gyro_init.c
View file

@ -32,6 +32,8 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/config.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/accgyro/accgyro_mpu.h"
@ -80,7 +82,7 @@
#define ACTIVE_GYRO (&gyro.gyroSensor1)
#endif
static gyroDetectionFlags_t gyroDetectionFlags = NO_GYROS_DETECTED;
static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK;
static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
{
@ -565,45 +567,54 @@ bool gyroInit(void)
break;
}
gyroDetectionFlags = NO_GYROS_DETECTED;
gyroDetectionFlags = GYRO_NONE_MASK;
uint8_t gyrosToScan = gyroConfig()->gyrosDetected;
gyro.gyroToUse = gyroConfig()->gyro_to_use;
gyro.gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
if (gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) {
gyroDetectionFlags |= DETECTED_GYRO_1;
if ((!gyrosToScan || (gyrosToScan & GYRO_1_MASK)) && gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) {
gyroDetectionFlags |= GYRO_1_MASK;
}
#if defined(USE_MULTI_GYRO)
if (gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) {
gyroDetectionFlags |= DETECTED_GYRO_2;
if ((!gyrosToScan || (gyrosToScan & GYRO_2_MASK)) && gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) {
gyroDetectionFlags |= GYRO_2_MASK;
}
#endif
if (gyroDetectionFlags == NO_GYROS_DETECTED) {
if (gyroDetectionFlags == GYRO_NONE_MASK) {
return false;
}
bool eepromWriteRequired = false;
if (!gyrosToScan) {
gyroConfigMutable()->gyrosDetected = gyroDetectionFlags;
eepromWriteRequired = true;
}
#if defined(USE_MULTI_GYRO)
if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & DETECTED_BOTH_GYROS) == DETECTED_BOTH_GYROS))
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1))
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) {
if (gyroDetectionFlags & DETECTED_GYRO_1) {
if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK))
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) {
if (gyroDetectionFlags & GYRO_1_MASK) {
gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
} else {
gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_2;
}
gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
eepromWriteRequired = true;
}
// Only allow using both gyros simultaneously if they are the same hardware type.
if (((gyroDetectionFlags & DETECTED_BOTH_GYROS) == DETECTED_BOTH_GYROS) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
gyroDetectionFlags |= DETECTED_DUAL_GYROS;
if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
gyroDetectionFlags |= GYRO_IDENTICAL_MASK;
} else if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
eepromWriteRequired = true;
}
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
@ -613,6 +624,10 @@ bool gyroInit(void)
}
#endif
if (eepromWriteRequired) {
writeEEPROM();
}
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0));
gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection;