bool detectGyro(uint16_t gyroLpf)
{
gyroAlign = ALIGN_DEFAULT;
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro, gyroLpf)) {
#ifdef NAZE
gyroAlign = CW0_DEG;
#endif
return true;
}
#endif
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef NAZE
gyroAlign = CW0_DEG;
#endif
return true;
}
#endif
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef NAZE
gyroAlign = CW0_DEG;
#endif
return true;
}
#endif
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef CC3D
gyroAlign = CW270_DEG;
#endif
return true;
}
#endif
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
return true;
}
#endif
return false;
}
STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
{
gyroHardware_e gyroHardware = GYRO_DEFAULT;
switch (gyroHardware) {
case GYRO_DEFAULT:
FALLTHROUGH;
#ifdef USE_GYRO_MPU6050
case GYRO_MPU6050:
if (mpu6050GyroDetect(dev)) {
gyroHardware = GYRO_MPU6050;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_L3G4200D
case GYRO_L3G4200D:
if (l3g4200dDetect(dev)) {
gyroHardware = GYRO_L3G4200D;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_MPU3050
case GYRO_MPU3050:
if (mpu3050Detect(dev)) {
gyroHardware = GYRO_MPU3050;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_L3GD20
case GYRO_L3GD20:
if (l3gd20GyroDetect(dev)) {
gyroHardware = GYRO_L3GD20;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_SPI_MPU6000
case GYRO_MPU6000:
if (mpu6000SpiGyroDetect(dev)) {
gyroHardware = GYRO_MPU6000;
break;
}
FALLTHROUGH;
#endif
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
case GYRO_MPU6500:
case GYRO_ICM20601:
case GYRO_ICM20602:
case GYRO_ICM20608G:
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
#else
if (mpu6500GyroDetect(dev)) {
#endif
switch (dev->mpuDetectionResult.sensor) {
case MPU_9250_SPI:
gyroHardware = GYRO_MPU9250;
break;
case ICM_20601_SPI:
gyroHardware = GYRO_ICM20601;
break;
case ICM_20602_SPI:
gyroHardware = GYRO_ICM20602;
break;
case ICM_20608_SPI:
gyroHardware = GYRO_ICM20608G;
break;
default:
gyroHardware = GYRO_MPU6500;
}
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_SPI_MPU9250
case GYRO_MPU9250:
if (mpu9250SpiGyroDetect(dev)) {
gyroHardware = GYRO_MPU9250;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_SPI_ICM20649
case GYRO_ICM20649:
if (icm20649SpiGyroDetect(dev)) {
gyroHardware = GYRO_ICM20649;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_SPI_ICM20689
case GYRO_ICM20689:
if (icm20689SpiGyroDetect(dev)) {
gyroHardware = GYRO_ICM20689;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_BMI160
case GYRO_BMI160:
if (bmi160SpiGyroDetect(dev)) {
gyroHardware = GYRO_BMI160;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_FAKE_GYRO
case GYRO_FAKE:
if (fakeGyroDetect(dev)) {
gyroHardware = GYRO_FAKE;
break;
}
FALLTHROUGH;
#endif
default:
gyroHardware = GYRO_NONE;
}
if (gyroHardware != GYRO_NONE) {
sensorsSet(SENSOR_GYRO);
}
return gyroHardware;
}
static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
{
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
mpuPreInit(config);
#else
UNUSED(config);
#endif
}
bool detectGyro(void)
{
bool sensorDetected;
UNUSED(sensorDetected); // avoid unused-variable warning on some targets.
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN
gyroHardware = GYRO_MPU6000;
gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6500:
#ifdef USE_GYRO_MPU6500
#ifdef USE_GYRO_SPI_MPU6500
sensorDetected = mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro);
#else
sensorDetected = mpu6500GyroDetect(&gyro);
#endif
if (sensorDetected) {
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
bool detectGyro(void)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN
gyroHardware = GYRO_MPU6000;
gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
#else
if (mpu6500GyroDetect(&gyro))
#endif
{
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU9250:
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiGyroDetect(&gyro))
{
gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN
gyroAlign = GYRO_MPU9250_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
bool detectGyro(uint16_t gyroLpf)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
gyroHardware = GYRO_SPI_MPU6000;
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6500:
#ifdef USE_GYRO_SPI_MPU6500
#ifdef USE_HARDWARE_REVISION_DETECTION
spiBusInit();
#endif
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
break;
}
#else
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
break;
}
#endif
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
static bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig)
{
dev->mpuIntExtiConfig = extiConfig;
gyroSensor_e gyroHardware = GYRO_AUTODETECT;
dev->gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_AUTODETECT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(dev)) {
gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
dev->gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(dev)) {
gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN
dev->gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(dev)) {
gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN
dev->gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(dev)) {
gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN
dev->gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(dev)) {
gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN
dev->gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
#else
if (mpu6500GyroDetect(dev)) {
#endif
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
dev->gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(dev)) {
gyroHardware = GYRO_FAKE;
break;
}
#endif
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
addBootlogEvent6(BOOT_EVENT_GYRO_DETECTION, BOOT_EVENT_FLAGS_NONE, gyroHardware, 0, 0, 0);
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
bool gyroInit(const gyroConfig_t *gyroConfigToUse)
{
gyroConfig = gyroConfigToUse;
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetect(&gyro.dev);
mpuReset = gyro.dev.mpuConfiguration.reset;
#endif
if (!gyroDetect(&gyro.dev, extiConfig)) {
return false;
}
// After refactoring this function is always called after gyro sampling rate is known, so
// no additional condition is required
// Set gyro sample rate before driver initialisation
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
// driver initialisation
gyro.dev.init(&gyro.dev);
if (gyroConfig->gyro_soft_lpf_hz) {
for (int axis = 0; axis < 3; axis++) {
#ifdef ASYNC_GYRO_PROCESSING
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, getGyroUpdateRate());
#else
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
#endif
}
}
return true;
}
