Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Moved sensor detection into respective sensor modules

Browse source
This commit is contained in:
Martin Budden 2016-12-05 15:26:14 +00:00
parent 4bee6193e8
commit 4bb6820c42
10 changed files with 535 additions and 508 deletions
Download patch file Download diff file Expand all files Collapse all files

160
src/main/sensors/gyro.c
View file

@ -27,8 +27,28 @@
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/accgyro.h"
#include "drivers/accgyro_adxl345.h"
#include "drivers/accgyro_bma280.h"
#include "drivers/accgyro_fake.h"
#include "drivers/accgyro_l3g4200d.h"
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_mpu6500.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
#include "drivers/bus_spi.h"
#include "drivers/accgyro_spi_icm20689.h"
#include "drivers/accgyro_spi_mpu6000.h"
#include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/accgyro_spi_mpu9250.h"
#include "drivers/gyro_sync.h"
#include "drivers/system.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"
#include "io/statusindicator.h"
@ -51,6 +71,143 @@ static void *notchFilter1[3];
static filterApplyFnPtr notchFilter2ApplyFn;
static void *notchFilter2[3];
bool gyroDetect(gyroDev_t *dev)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyro.dev.gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(dev)) {
gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
gyro.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
gyro.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
gyro.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
gyro.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
gyro.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
gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_MPU9250:
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiGyroDetect(dev))
{
gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN
gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_ICM20689:
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiGyroDetect(dev))
{
gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN
gyro.dev.gyroAlign = GYRO_ICM20689_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;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
void gyroInit(const gyroConfig_t *gyroConfigToUse)
{
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
@ -59,6 +216,9 @@ void gyroInit(const gyroConfig_t *gyroConfigToUse)
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.dev.init(&gyro.dev);
gyroConfig = gyroConfigToUse;
softLpfFilterApplyFn = nullFilterApply;