Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

accgyro betaflight catchup

Browse source
This commit is contained in:
Martin Budden 2017-08-30 05:56:25 +01:00
parent 49c35fbaf8
commit b870c1a8a2
22 changed files with 115 additions and 104 deletions
Download patch file Download diff file Expand all files Collapse all files

19
src/main/drivers/accgyro/accgyro.h
View file

@ -17,6 +17,7 @@
#pragma once #pragma once
#include "platform.h"
#include "common/axis.h" #include "common/axis.h"
#include "drivers/exti.h" #include "drivers/exti.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
@ -42,14 +43,14 @@ typedef enum {
} gyroRateKHz_e; } gyroRateKHz_e;
typedef struct gyroDev_s { typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function sensorGyroInitFuncPtr initFn; // initialize function
sensorGyroReadFuncPtr read; // read 3 axis data function sensorGyroReadFuncPtr readFn; // read 3 axis data function
sensorGyroReadDataFuncPtr temperature; // read temperature if available sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus; sensorGyroInterruptStatusFuncPtr intStatusFn;
sensorGyroUpdateFuncPtr update; sensorGyroUpdateFuncPtr updateFn;
extiCallbackRec_t exti; extiCallbackRec_t exti;
busDevice_t bus; busDevice_t bus;
float scale; // scalefactor float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int16_t gyroZero[XYZ_AXIS_COUNT]; int16_t gyroZero[XYZ_AXIS_COUNT];
uint8_t lpf; uint8_t lpf;
@ -63,12 +64,12 @@ typedef struct gyroDev_s {
} gyroDev_t; } gyroDev_t;
typedef struct accDev_s { typedef struct accDev_s {
sensorAccInitFuncPtr init; // initialize function sensorAccInitFuncPtr initFn; // initialize function
sensorAccReadFuncPtr read; // read 3 axis data function sensorAccReadFuncPtr readFn; // read 3 axis data function
busDevice_t bus; busDevice_t bus;
uint16_t acc_1G; uint16_t acc_1G;
int16_t ADCRaw[XYZ_AXIS_COUNT]; int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known char revisionCode; // a revision code for the sensor, if known
sensor_align_e accAlign; sensor_align_e accAlign;
mpuDetectionResult_t mpuDetectionResult; mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration; mpuConfiguration_t mpuConfiguration;

4
src/main/drivers/accgyro/accgyro_adxl345.c
View file

@ -129,7 +129,7 @@ bool adxl345Detect(accDev_t *acc, drv_adxl345_config_t *init)
// use ADXL345's fifo to filter data or not // use ADXL345's fifo to filter data or not
useFifo = init->useFifo; useFifo = init->useFifo;
acc->init = adxl345Init; acc->initFn = adxl345Init;
acc->read = adxl345Read; acc->readFn = adxl345Read;
return true; return true;
} }

4
src/main/drivers/accgyro/accgyro_bma280.c
View file

@ -65,7 +65,7 @@ bool bma280Detect(accDev_t *acc)
if (!ack || sig != 0xFB) if (!ack || sig != 0xFB)
return false; return false;
acc->init = bma280Init; acc->initFn = bma280Init;
acc->read = bma280Read; acc->readFn = bma280Read;
return true; return true;
} }

12
src/main/drivers/accgyro/accgyro_fake.c
View file

@ -66,10 +66,10 @@ static bool fakeGyroInitStatus(gyroDev_t *gyro)
bool fakeGyroDetect(gyroDev_t *gyro) bool fakeGyroDetect(gyroDev_t *gyro)
{ {
gyro->init = fakeGyroInit; gyro->initFn = fakeGyroInit;
gyro->intStatus = fakeGyroInitStatus; gyro->intStatusFn = fakeGyroInitStatus;
gyro->read = fakeGyroRead; gyro->readFn = fakeGyroRead;
gyro->temperature = fakeGyroReadTemperature; gyro->temperatureFn = fakeGyroReadTemperature;
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
return true; return true;
} }
@ -102,8 +102,8 @@ static bool fakeAccRead(accDev_t *acc)
bool fakeAccDetect(accDev_t *acc) bool fakeAccDetect(accDev_t *acc)
{ {
acc->init = fakeAccInit; acc->initFn = fakeAccInit;
acc->read = fakeAccRead; acc->readFn = fakeAccRead;
acc->revisionCode = 0; acc->revisionCode = 0;
return true; return true;
} }

4
src/main/drivers/accgyro/accgyro_l3g4200d.c
View file

@ -113,8 +113,8 @@ bool l3g4200dDetect(gyroDev_t *gyro)
if (deviceid != L3G4200D_ID) if (deviceid != L3G4200D_ID)
return false; return false;
gyro->init = l3g4200dInit; gyro->initFn = l3g4200dInit;
gyro->read = l3g4200dRead; gyro->readFn = l3g4200dRead;
// 14.2857dps/lsb scalefactor // 14.2857dps/lsb scalefactor
gyro->scale = 1.0f / 14.2857f; gyro->scale = 1.0f / 14.2857f;

4
src/main/drivers/accgyro/accgyro_l3gd20.c
View file

@ -152,8 +152,8 @@ static bool l3gd20GyroRead(gyroDev_t *gyro)
bool l3gd20Detect(gyroDev_t *gyro) bool l3gd20Detect(gyroDev_t *gyro)
{ {
gyro->init = l3gd20GyroInit; gyro->initFn = l3gd20GyroInit;
gyro->read = l3gd20GyroRead; gyro->readFn = l3gd20GyroRead;
gyro->scale = L3GD20_GYRO_SCALE_FACTOR; gyro->scale = L3GD20_GYRO_SCALE_FACTOR;

4
src/main/drivers/accgyro/accgyro_lsm303dlhc.c
View file

@ -166,8 +166,8 @@ bool lsm303dlhcAccDetect(accDev_t *acc)
if (!ack) if (!ack)
return false; return false;
acc->init = lsm303dlhcAccInit; acc->initFn = lsm303dlhcAccInit;
acc->read = lsm303dlhcAccRead; acc->readFn = lsm303dlhcAccRead;
return true; return true;
} }
#endif #endif

4
src/main/drivers/accgyro/accgyro_mma845x.c
View file

@ -135,8 +135,8 @@ bool mma8452Detect(accDev_t *acc)
if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE)) if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE))
return false; return false;
acc->init = mma8452Init; acc->initFn = mma8452Init;
acc->read = mma8452Read; acc->readFn = mma8452Read;
device_id = sig; device_id = sig;
return true; return true;
} }

62
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -27,14 +27,17 @@
#include "build/debug.h" #include "build/debug.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h"
#include "drivers/nvic.h" #include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/system.h" #include "drivers/exti.h"
#include "drivers/time.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/exti.h" #include "drivers/exti.h"
#include "drivers/bus_i2c.h" #include "drivers/nvic.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro.h"
@ -56,10 +59,6 @@ mpuResetFnPtr mpuResetFn;
#define MPU_ADDRESS 0x68 #define MPU_ADDRESS 0x68
// WHO_AM_I register contents for MPU3050, 6050 and 6500
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPUx0x0_WHO_AM_I_CONST (0x68)
#define MPU_INQUIRY_MASK 0x7E #define MPU_INQUIRY_MASK 0x7E
static void mpu6050FindRevision(gyroDev_t *gyro) static void mpu6050FindRevision(gyroDev_t *gyro)
@ -115,8 +114,8 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
#endif #endif
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
gyro->dataReady = true; gyro->dataReady = true;
if (gyro->update) { if (gyro->updateFn) {
gyro->update(gyro); gyro->updateFn(gyro);
} }
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
const uint32_t now2Us = micros(); const uint32_t now2Us = micros();
@ -161,14 +160,14 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data) bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data)
{ {
UNUSED(bus); UNUSED(bus);
bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); const bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
return ack; return ack;
} }
bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data) bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data)
{ {
UNUSED(bus); UNUSED(bus);
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); const bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
return ack; return ack;
} }
@ -176,7 +175,7 @@ bool mpuAccRead(accDev_t *acc)
{ {
uint8_t data[6]; uint8_t data[6];
bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, 6, data); const bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) { if (!ack) {
return false; return false;
} }
@ -191,7 +190,7 @@ bool mpuAccRead(accDev_t *acc)
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn) void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
{ {
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) { ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
gyro->update = updateFn; gyro->updateFn = updateFn;
} }
} }
@ -227,11 +226,16 @@ bool mpuCheckDataReady(gyroDev_t* gyro)
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{ {
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
uint8_t sensor = MPU_NONE;
UNUSED(sensor);
#ifdef USE_GYRO_SPI_MPU6000 #ifdef USE_GYRO_SPI_MPU6000
#ifdef MPU6000_CS_PIN #ifdef MPU6000_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
#endif #endif
if (mpu6000SpiDetect(&gyro->bus)) { sensor = mpu6000SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister; gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
@ -241,11 +245,13 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_MPU6500 #ifdef USE_GYRO_SPI_MPU6500
#ifdef MPU6500_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus); #endif
sensor = mpu6500SpiDetect(&gyro->bus);
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
if (mpu6500Sensor != MPU_NONE) { if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister; gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister; gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
@ -254,13 +260,14 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_MPU9250 #ifdef USE_GYRO_SPI_MPU9250
#ifdef MPU9250_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin;
if (mpu9250SpiDetect(&gyro->bus)) { #endif
sensor = mpu9250SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister; gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister;
gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister;
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister; gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro; gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
return true; return true;
@ -268,8 +275,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_ICM20608 #ifdef USE_GYRO_SPI_ICM20608
#ifdef ICM20608_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20608_CS_PIN)) : gyro->bus.spi.csnPin; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20608_CS_PIN)) : gyro->bus.spi.csnPin;
if (icm20608SpiDetect(&gyro->bus)) { #endif
sensor = icm20608SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
mpuDetectionResult.sensor = ICM_20608_SPI; mpuDetectionResult.sensor = ICM_20608_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.readFn = icm20608SpiReadRegister; mpuConfiguration.readFn = icm20608SpiReadRegister;
@ -279,8 +289,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_ICM20689 #ifdef USE_GYRO_SPI_ICM20689
#ifdef ICM20689_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin; gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin;
if (icm20689SpiDetect(&gyro->bus)) { #endif
sensor = icm20689SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689SpiReadRegister; gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
@ -289,7 +302,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
} }
#endif #endif
UNUSED(gyro);
return false; return false;
} }
#endif #endif

17
src/main/drivers/accgyro/accgyro_mpu.h
View file

@ -30,6 +30,18 @@
#define MPU_RA_WHO_AM_I 0x75 #define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00 #define MPU_RA_WHO_AM_I_LEGACY 0x00
#define MPUx0x0_WHO_AM_I_CONST (0x68) // MPU3050, 6000 and 6050
#define MPU6000_WHO_AM_I_CONST (0x68)
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPU9250_WHO_AM_I_CONST (0x71)
#define MPU9255_WHO_AM_I_CONST (0x73)
#define ICM20601_WHO_AM_I_CONST (0xAC)
#define ICM20602_WHO_AM_I_CONST (0x12)
#define ICM20608G_WHO_AM_I_CONST (0xAF)
#define ICM20689_WHO_AM_I_CONST (0x98)
// RA = Register Address // RA = Register Address
#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD #define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
@ -126,15 +138,13 @@
typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data); typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data); typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
typedef void(*mpuResetFnPtr)(void); typedef void (*mpuResetFnPtr)(void);
extern mpuResetFnPtr mpuResetFn; extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s { typedef struct mpuConfiguration_s {
mpuReadRegisterFnPtr readFn; mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFnPtr writeFn; mpuWriteRegisterFnPtr writeFn;
mpuReadRegisterFnPtr slowreadFn;
mpuWriteRegisterFnPtr verifywriteFn;
mpuResetFnPtr resetFn; mpuResetFnPtr resetFn;
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t; } mpuConfiguration_t;
@ -204,4 +214,3 @@ bool mpuGyroRead(struct gyroDev_s *gyro);
void mpuDetect(struct gyroDev_s *gyro); void mpuDetect(struct gyroDev_s *gyro);
bool mpuCheckDataReady(struct gyroDev_s *gyro); bool mpuCheckDataReady(struct gyroDev_s *gyro);
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn); void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);

8
src/main/drivers/accgyro/accgyro_mpu3050.c
View file

@ -80,10 +80,10 @@ bool mpu3050Detect(gyroDev_t *gyro)
if (gyro->mpuDetectionResult.sensor != MPU_3050) { if (gyro->mpuDetectionResult.sensor != MPU_3050) {
return false; return false;
} }
gyro->init = mpu3050Init; gyro->initFn = mpu3050Init;
gyro->read = mpuGyroRead; gyro->readFn = mpuGyroRead;
gyro->temperature = mpu3050ReadTemperature; gyro->temperatureFn = mpu3050ReadTemperature;
gyro->intStatus = mpuCheckDataReady; gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;

10
src/main/drivers/accgyro/accgyro_mpu6050.c
View file

@ -68,8 +68,8 @@ bool mpu6050AccDetect(accDev_t *acc)
return false; return false;
} }
acc->init = mpu6050AccInit; acc->initFn = mpu6050AccInit;
acc->read = mpuAccRead; acc->readFn = mpuAccRead;
acc->revisionCode = (acc->mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. acc->revisionCode = (acc->mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
return true; return true;
@ -105,9 +105,9 @@ bool mpu6050GyroDetect(gyroDev_t *gyro)
if (gyro->mpuDetectionResult.sensor != MPU_60x0) { if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
} }
gyro->init = mpu6050GyroInit; gyro->initFn = mpu6050GyroInit;
gyro->read = mpuGyroRead; gyro->readFn = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady; gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;

10
src/main/drivers/accgyro/accgyro_mpu6500.c
View file

@ -46,8 +46,8 @@ bool mpu6500AccDetect(accDev_t *acc)
return false; return false;
} }
acc->init = mpu6500AccInit; acc->initFn = mpu6500AccInit;
acc->read = mpuAccRead; acc->readFn = mpuAccRead;
return true; return true;
} }
@ -107,9 +107,9 @@ bool mpu6500GyroDetect(gyroDev_t *gyro)
return false; return false;
} }
gyro->init = mpu6500GyroInit; gyro->initFn = mpu6500GyroInit;
gyro->read = mpuGyroRead; gyro->readFn = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady; gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;

6
src/main/drivers/accgyro/accgyro_mpu6500.h
View file

@ -17,12 +17,6 @@
#pragma once #pragma once
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPU9250_WHO_AM_I_CONST (0x71)
#define ICM20608G_WHO_AM_I_CONST (0xAF)
#define ICM20602_WHO_AM_I_CONST (0x12)
#define ICM20689_WHO_AM_I_CONST (0x98)
#define MPU6500_BIT_RESET (0x80) #define MPU6500_BIT_RESET (0x80)
#define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4) #define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4)
#define MPU6500_BIT_BYPASS_EN (1 << 0) #define MPU6500_BIT_BYPASS_EN (1 << 0)

10
src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View file

@ -258,8 +258,8 @@ bool mpu6000SpiAccDetect(accDev_t *acc)
return false; return false;
} }
acc->init = mpu6000SpiAccInit; acc->initFn = mpu6000SpiAccInit;
acc->read = mpuAccRead; acc->readFn = mpuAccRead;
return true; return true;
} }
@ -270,9 +270,9 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
return false; return false;
} }
gyro->init = mpu6000SpiGyroInit; gyro->initFn = mpu6000SpiGyroInit;
gyro->read = mpuGyroRead; gyro->readFn = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady; gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;

2
src/main/drivers/accgyro/accgyro_spi_mpu6000.h
View file

@ -12,8 +12,6 @@
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS) #define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
#define MPU6000_WHO_AM_I_CONST (0x68)
// RF = Register Flag // RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0) #define MPU_RF_DATA_RDY_EN (1 << 0)

10
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View file

@ -125,8 +125,8 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
return false; return false;
} }
acc->init = mpu6500SpiAccInit; acc->initFn = mpu6500SpiAccInit;
acc->read = mpuAccRead; acc->readFn = mpuAccRead;
return true; return true;
} }
@ -156,9 +156,9 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
return false; return false;
} }
gyro->init = mpu6500SpiGyroInit; gyro->initFn = mpu6500SpiGyroInit;
gyro->read = mpuGyroRead; gyro->readFn = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady; gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;

10
src/main/drivers/accgyro/accgyro_spi_mpu9250.c
View file

@ -193,8 +193,8 @@ bool mpu9250SpiAccDetect(accDev_t *acc)
return false; return false;
} }
acc->init = mpu9250SpiAccInit; acc->initFn = mpu9250SpiAccInit;
acc->read = mpuAccRead; acc->readFn = mpuAccRead;
return true; return true;
} }
@ -223,9 +223,9 @@ bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
return false; return false;
} }
gyro->init = mpu9250SpiGyroInit; gyro->initFn = mpu9250SpiGyroInit;
gyro->read = mpuGyroRead; gyro->readFn = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady; gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;

3
src/main/drivers/accgyro/accgyro_spi_mpu9250.h
View file

@ -18,9 +18,6 @@
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS) #define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
#define MPU9250_WHO_AM_I_CONST (0x71)
#define MPU9255_WHO_AM_I_CONST (0x73)
#define MPU9250_BIT_RESET (0x80) #define MPU9250_BIT_RESET (0x80)
// RF = Register Flag // RF = Register Flag

4
src/main/drivers/gyro_sync.c
View file

@ -26,9 +26,9 @@
bool gyroSyncCheckIntStatus(gyroDev_t *gyro) bool gyroSyncCheckIntStatus(gyroDev_t *gyro)
{ {
if (!gyro->intStatus) if (!gyro->intStatusFn)
return false; return false;
return gyro->intStatus(gyro); return gyro->intStatusFn(gyro);
} }
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator) uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)

4
src/main/sensors/acceleration.c
View file

@ -289,7 +289,7 @@ bool accInit(uint32_t targetLooptime)
return false; return false;
} }
acc.dev.acc_1G = 256; // set default acc.dev.acc_1G = 256; // set default
acc.dev.init(&acc.dev); acc.dev.initFn(&acc.dev);
acc.accTargetLooptime = targetLooptime; acc.accTargetLooptime = targetLooptime;
accInitFilters(); accInitFilters();
if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
@ -449,7 +449,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
void accUpdate(void) void accUpdate(void)
{ {
if (!acc.dev.read(&acc.dev)) { if (!acc.dev.readFn(&acc.dev)) {
return; return;
} }

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

@ -292,7 +292,7 @@ bool gyroInit(void)
gyroDev0.lpf = gyroConfig()->gyro_lpf; gyroDev0.lpf = gyroConfig()->gyro_lpf;
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->looptime, gyroConfig()->gyro_lpf, gyroConfig()->gyroSync, gyroConfig()->gyroSyncDenominator); gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->looptime, gyroConfig()->gyro_lpf, gyroConfig()->gyroSync, gyroConfig()->gyroSyncDenominator);
// driver initialisation // driver initialisation
gyroDev0.init(&gyroDev0); gyroDev0.initFn(&gyroDev0);
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
gyroDev0.gyroAlign = gyroConfig()->gyro_align; gyroDev0.gyroAlign = gyroConfig()->gyro_align;
@ -405,7 +405,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t
void gyroUpdate(void) void gyroUpdate(void)
{ {
// range: +/- 8192; +/- 2000 deg/sec // range: +/- 8192; +/- 2000 deg/sec
if (gyroDev0.read(&gyroDev0)) { if (gyroDev0.readFn(&gyroDev0)) {
if (isCalibrationComplete(&gyroCalibration)) { if (isCalibrationComplete(&gyroCalibration)) {
// Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X]; gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
@ -447,8 +447,8 @@ void gyroUpdate(void)
void gyroReadTemperature(void) void gyroReadTemperature(void)
{ {
if (gyroDev0.temperature) { if (gyroDev0.temperatureFn) {
gyroDev0.temperature(&gyroDev0, &gyroTemperature0); gyroDev0.temperatureFn(&gyroDev0, &gyroTemperature0);
} }
} }