Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 09:45:37 +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

12
src/main/sensors/gyro.h
View file

@ -49,12 +49,12 @@ typedef union gyroLowpassFilter_u {
} gyroLowpassFilter_t;
typedef enum gyroDetectionFlags_e {
NO_GYROS_DETECTED = 0,
DETECTED_GYRO_1 = (1 << 0),
GYRO_NONE_MASK = 0,
GYRO_1_MASK = BIT(0),
#if defined(USE_MULTI_GYRO)
DETECTED_GYRO_2 = (1 << 1),
DETECTED_BOTH_GYROS = (DETECTED_GYRO_1 | DETECTED_GYRO_2),
DETECTED_DUAL_GYROS = (1 << 7), // All gyros are of the same hardware type
GYRO_2_MASK = BIT(1),
GYRO_ALL_MASK = (GYRO_1_MASK | GYRO_2_MASK),
GYRO_IDENTICAL_MASK = BIT(7), // All gyros are of the same hardware type
#endif
} gyroDetectionFlags_t;
@ -195,6 +195,8 @@ typedef struct gyroConfig_s {
uint16_t dyn_notch_min_hz;
uint8_t gyro_filter_debug_axis;
uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);

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;