rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

LPF setting is not needed to detect the gyro sensor, only when it's 

initialised; now the lpf setting is passed to gyroInit().

This saves a bit of code size and ram as well as making the code
cleaner.
This commit is contained in:
Dominic Clifton 2015-09-21 14:16:21 +01:00
parent 06ceac0614
commit aac13914f9
20 changed files with 83 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/drivers/accgyro.h
View File

@ -20,7 +20,7 @@
extern uint16_t acc_1G; // FIXME move into acc_t
typedef struct gyro_s {
sensorInitFuncPtr init; // initialize function
sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor

23
src/main/drivers/accgyro_l3g4200d.c
View File

@ -54,12 +54,10 @@
#define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
static void l3g4200dInit(void);
static void l3g4200dInit(uint16_t lpf);
static bool l3g4200dRead(int16_t *gyroADC);
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
bool l3g4200dDetect(gyro_t *gyro)
{
uint8_t deviceid;
@ -75,7 +73,15 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
// 14.2857dps/lsb scalefactor
gyro->scale = 1.0f / 14.2857f;
// default LPF is set to 32Hz
return true;
}
static void l3g4200dInit(uint16_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
switch (lpf) {
default:
case 32:
@ -92,13 +98,6 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
break;
}
return true;
}
static void l3g4200dInit(void)
{
bool ack;
delay(100);
ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);

2
src/main/drivers/accgyro_l3g4200d.h
View File

@ -17,4 +17,4 @@
#pragma once
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf);
bool l3g4200dDetect(gyro_t *gyro);

8
src/main/drivers/accgyro_l3gd20.c
View File

@ -86,8 +86,10 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER);
}
void l3gd20GyroInit(void)
void l3gd20GyroInit(uint16_t lpf)
{
UNUSED(lpf); // FIXME use it!
l3gd20SpiInit(L3GD20_SPI);
GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
@ -150,10 +152,8 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf)
bool l3gd20Detect(gyro_t *gyro)
{
UNUSED(lpf);
gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead;

2
src/main/drivers/accgyro_l3gd20.h
View File

@ -17,4 +17,4 @@
#pragma once
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf);
bool l3gd20Detect(gyro_t *gyro);

10
src/main/drivers/accgyro_mpu.c
View File

@ -59,10 +59,6 @@ mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
static const extiConfig_t *mpuIntExtiConfig = NULL;
// FIXME move into mpuConfiguration
uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
#define MPU_ADDRESS 0x68
// MPU6050
@ -299,8 +295,10 @@ void mpuIntExtiInit(void)
mpuExtiInitDone = true;
}
void configureMPULPF(uint16_t lpf)
uint8_t determineMPULPF(uint16_t lpf)
{
uint8_t mpuLowPassFilter;
if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188)
@ -317,6 +315,8 @@ void configureMPULPF(uint16_t lpf)
mpuLowPassFilter = INV_FILTER_5HZ;
else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return mpuLowPassFilter;
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)

4
src/main/drivers/accgyro_mpu.h
View File

@ -78,7 +78,9 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;
void configureMPULPF(uint16_t lpf);
extern mpuDetectionResult_t mpuDetectionResult;
uint8_t determineMPULPF(uint16_t lpf);
void configureMPUDataReadyInterruptHandling(void);
void mpuIntExtiInit(void);
bool mpuAccRead(int16_t *accData);

13
src/main/drivers/accgyro_mpu3050.c
View File

@ -31,9 +31,6 @@
#include "accgyro_mpu.h"
#include "accgyro_mpu3050.h"
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
// MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68
@ -49,10 +46,10 @@ extern uint8_t mpuLowPassFilter;
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
void mpu3050Init(void);
static void mpu3050Init(uint16_t lpf);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
bool mpu3050Detect(gyro_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_3050) {
return false;
@ -64,15 +61,15 @@ bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true;
}
void mpu3050Init(void)
static void mpu3050Init(uint16_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);

3
src/main/drivers/accgyro_mpu3050.h
View File

@ -26,5 +26,4 @@
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
void mpu3050Init(void);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf);
bool mpu3050Detect(gyro_t *gyro);

14
src/main/drivers/accgyro_mpu6050.c
View File

@ -37,7 +37,6 @@
#include "accgyro_mpu.h"
#include "accgyro_mpu6050.h"
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
//#define DEBUG_MPU_DATA_READY_INTERRUPT
@ -146,7 +145,7 @@ extern uint8_t mpuLowPassFilter;
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
static void mpu6050AccInit(void);
static void mpu6050GyroInit(void);
static void mpu6050GyroInit(uint16_t lpf);
bool mpu6050AccDetect(acc_t *acc)
{
@ -161,7 +160,7 @@ bool mpu6050AccDetect(acc_t *acc)
return true;
}
bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
bool mpu6050GyroDetect(gyro_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;
@ -172,8 +171,6 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true;
}
@ -191,11 +188,14 @@ static void mpu6050AccInit(void)
}
}
static void mpu6050GyroInit(void)
static void mpu6050GyroInit(uint16_t lpf)
{
bool ack;
mpuIntExtiInit();
bool ack;
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)

2
src/main/drivers/accgyro_mpu6050.h
View File

@ -18,4 +18,4 @@
#pragma once
bool mpu6050AccDetect(acc_t *acc);
bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6050GyroDetect(gyro_t *gyro);

13
src/main/drivers/accgyro_mpu6500.c
View File

@ -33,13 +33,8 @@
#include "accgyro_mpu.h"
#include "accgyro_mpu6500.h"
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
extern uint16_t acc_1G;
bool mpu6500AccDetect(acc_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
@ -52,7 +47,7 @@ bool mpu6500AccDetect(acc_t *acc)
return true;
}
bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf)
bool mpu6500GyroDetect(gyro_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
@ -64,8 +59,6 @@ bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true;
}
@ -74,7 +67,7 @@ void mpu6500AccInit(void)
acc_1G = 512 * 8;
}
void mpu6500GyroInit(void)
void mpu6500GyroInit(uint16_t lpf)
{
#ifdef NAZE
// FIXME target specific code in driver code.
@ -89,6 +82,8 @@ void mpu6500GyroInit(void)
}
#endif
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07);

4
src/main/drivers/accgyro_mpu6500.h
View File

@ -36,7 +36,7 @@
#pragma once
bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6500GyroDetect(gyro_t *gyro);
void mpu6500AccInit(void);
void mpu6500GyroInit(void);
void mpu6500GyroInit(uint16_t lpf);

9
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -43,7 +43,6 @@
static void mpu6000AccAndGyroInit(void);
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
static bool mpuSpi6000InitDone = false;
@ -121,8 +120,10 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true;
}
void mpu6000SpiGyroInit(void)
void mpu6000SpiGyroInit(uint16_t lpf)
{
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
@ -249,7 +250,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
return true;
}
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
bool mpu6000SpiGyroDetect(gyro_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
@ -261,7 +262,5 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true;
}

2
src/main/drivers/accgyro_spi_mpu6000.h
View File

@ -47,7 +47,7 @@
bool mpu6000SpiDetect(void);
bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6000SpiGyroDetect(gyro_t *gyro);
bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

7
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -38,9 +38,6 @@
#define DISABLE_MPU6500 GPIO_SetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
#define ENABLE_MPU6500 GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
extern uint16_t acc_1G;
bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
@ -128,7 +125,7 @@ bool mpu6500SpiAccDetect(acc_t *acc)
return true;
}
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
bool mpu6500SpiGyroDetect(gyro_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
@ -140,8 +137,6 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
configureMPULPF(lpf);
return true;
}

2
src/main/drivers/accgyro_spi_mpu6500.h
View File

@ -38,7 +38,7 @@
bool mpu6500SpiDetect(void);
bool mpu6500SpiAccDetect(acc_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6500SpiGyroDetect(gyro_t *gyro);
bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

1
src/main/drivers/sensor.h
View File

@ -19,3 +19,4 @@
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf); // gyro sensor init prototype

7
src/main/flight/mixer.h
View File

@ -194,16 +194,19 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct escAndServoConfig_s;
struct rxConfig_s;
void mixerUseConfigs(
#ifdef USE_SERVOS
servoParam_t *servoConfToUse,
struct gimbalConfig_s *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse);
struct rxConfig_s *rxConfigToUse);
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers);

40
src/main/sensors/initialisation.c
View File

@ -128,19 +128,25 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
}
#ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {}
static bool fakeGyroRead(int16_t *gyroADC) {
static void fakeGyroInit(uint16_t lpf)
{
UNUSED(lpf);
}
static bool fakeGyroRead(int16_t *gyroADC)
{
memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
return true;
}
static bool fakeGyroReadTemp(int16_t *tempData) {
static bool fakeGyroReadTemp(int16_t *tempData)
{
UNUSED(tempData);
return true;
}
bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
bool fakeGyroDetect(gyro_t *gyro)
{
UNUSED(lpf);
gyro->init = fakeGyroInit;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
@ -164,7 +170,7 @@ bool fakeAccDetect(acc_t *acc)
}
#endif
bool detectGyro(uint16_t gyroLpf)
bool detectGyro(void)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
@ -175,7 +181,7 @@ bool detectGyro(uint16_t gyroLpf)
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro, gyroLpf)) {
if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
@ -186,7 +192,7 @@ bool detectGyro(uint16_t gyroLpf)
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
@ -198,7 +204,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
@ -210,7 +216,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
@ -222,7 +228,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN
gyroHardware = GYRO_MPU6000;
gyroAlign = GYRO_MPU6000_ALIGN;
@ -237,7 +243,7 @@ bool detectGyro(uint16_t gyroLpf)
#ifdef USE_HARDWARE_REVISION_DETECTION
spiBusInit();
#endif
if (mpu6500GyroDetect(&gyro, gyroLpf) || mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6500_ALIGN
gyroHardware = GYRO_MPU6500;
gyroAlign = GYRO_MPU6500_ALIGN;
@ -249,7 +255,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
if (fakeGyroDetect(&gyro)) {
gyroHardware = GYRO_FAKE;
break;
}
@ -403,7 +409,9 @@ retry:
static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifdef BARO
#ifndef BARO
UNUSED(baroHardwareToUse);
#else
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = baroHardwareToUse;
@ -603,7 +611,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro(gyroLpf)) {
if (!detectGyro()) {
return false;
}
detectAcc(accHardwareToUse);
@ -614,7 +622,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init();
gyro.init(gyroLpf);
detectMag(magHardwareToUse);