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
#include "platform.h"
#include "common/axis.h"
#include "drivers/exti.h"
#include "drivers/sensor.h"
@ -42,14 +43,14 @@ typedef enum {
} gyroRateKHz_e;
typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function
sensorGyroReadFuncPtr read; // read 3 axis data function
sensorGyroReadDataFuncPtr temperature; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus;
sensorGyroUpdateFuncPtr update;
sensorGyroInitFuncPtr initFn; // initialize function
sensorGyroReadFuncPtr readFn; // read 3 axis data function
sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatusFn;
sensorGyroUpdateFuncPtr updateFn;
extiCallbackRec_t exti;
busDevice_t bus;
float scale; // scalefactor
float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int16_t gyroZero[XYZ_AXIS_COUNT];
uint8_t lpf;
@ -63,12 +64,12 @@ typedef struct gyroDev_s {
} gyroDev_t;
typedef struct accDev_s {
sensorAccInitFuncPtr init; // initialize function
sensorAccReadFuncPtr read; // read 3 axis data function
sensorAccInitFuncPtr initFn; // initialize function
sensorAccReadFuncPtr readFn; // read 3 axis data function
busDevice_t bus;
uint16_t acc_1G;
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;
mpuDetectionResult_t mpuDetectionResult;
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
useFifo = init->useFifo;
acc->init = adxl345Init;
acc->read = adxl345Read;
acc->initFn = adxl345Init;
acc->readFn = adxl345Read;
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)
return false;
acc->init = bma280Init;
acc->read = bma280Read;
acc->initFn = bma280Init;
acc->readFn = bma280Read;
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)
{
gyro->init = fakeGyroInit;
gyro->intStatus = fakeGyroInitStatus;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemperature;
gyro->initFn = fakeGyroInit;
gyro->intStatusFn = fakeGyroInitStatus;
gyro->readFn = fakeGyroRead;
gyro->temperatureFn = fakeGyroReadTemperature;
gyro->scale = 1.0f / 16.4f;
return true;
}
@ -102,8 +102,8 @@ static bool fakeAccRead(accDev_t *acc)
bool fakeAccDetect(accDev_t *acc)
{
acc->init = fakeAccInit;
acc->read = fakeAccRead;
acc->initFn = fakeAccInit;
acc->readFn = fakeAccRead;
acc->revisionCode = 0;
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)
return false;
gyro->init = l3g4200dInit;
gyro->read = l3g4200dRead;
gyro->initFn = l3g4200dInit;
gyro->readFn = l3g4200dRead;
// 14.2857dps/lsb scalefactor
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)
{
gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead;
gyro->initFn = l3gd20GyroInit;
gyro->readFn = l3gd20GyroRead;
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)
return false;
acc->init = lsm303dlhcAccInit;
acc->read = lsm303dlhcAccRead;
acc->initFn = lsm303dlhcAccInit;
acc->readFn = lsm303dlhcAccRead;
return true;
}
#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))
return false;
acc->init = mma8452Init;
acc->read = mma8452Read;
acc->initFn = mma8452Init;
acc->readFn = mma8452Read;
device_id = sig;
return true;
}

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

@ -27,14 +27,17 @@
#include "build/debug.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/nvic.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.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/accgyro/accgyro.h"
@ -56,10 +59,6 @@ mpuResetFnPtr mpuResetFn;
#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
static void mpu6050FindRevision(gyroDev_t *gyro)
@ -115,8 +114,8 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
#endif
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
gyro->dataReady = true;
if (gyro->update) {
gyro->update(gyro);
if (gyro->updateFn) {
gyro->updateFn(gyro);
}
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
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)
{
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;
}
bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
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;
}
@ -176,7 +175,7 @@ bool mpuAccRead(accDev_t *acc)
{
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) {
return false;
}
@ -191,7 +190,7 @@ bool mpuAccRead(accDev_t *acc)
void mpuGyroSetIsrUpdate(gyroDev_t *gyro, sensorGyroUpdateFuncPtr updateFn)
{
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)
{
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 MPU6000_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
#endif
if (mpu6000SpiDetect(&gyro->bus)) {
sensor = mpu6000SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
@ -241,11 +245,13 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif
#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;
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
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
@ -254,13 +260,14 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif
#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;
if (mpu9250SpiDetect(&gyro->bus)) {
#endif
sensor = mpu9250SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister;
gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister;
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
return true;
@ -268,8 +275,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif
#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;
if (icm20608SpiDetect(&gyro->bus)) {
#endif
sensor = icm20608SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
mpuDetectionResult.sensor = ICM_20608_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.readFn = icm20608SpiReadRegister;
@ -279,8 +289,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif
#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;
if (icm20689SpiDetect(&gyro->bus)) {
#endif
sensor = icm20689SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
@ -289,7 +302,6 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
}
#endif
UNUSED(gyro);
return false;
}
#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_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
#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 (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
typedef void(*mpuResetFnPtr)(void);
typedef void (*mpuResetFnPtr)(void);
extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s {
mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFnPtr writeFn;
mpuReadRegisterFnPtr slowreadFn;
mpuWriteRegisterFnPtr verifywriteFn;
mpuResetFnPtr resetFn;
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;
@ -204,4 +214,3 @@ bool mpuGyroRead(struct gyroDev_s *gyro);
void mpuDetect(struct gyroDev_s *gyro);
bool mpuCheckDataReady(struct gyroDev_s *gyro);
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) {
return false;
}
gyro->init = mpu3050Init;
gyro->read = mpuGyroRead;
gyro->temperature = mpu3050ReadTemperature;
gyro->intStatus = mpuCheckDataReady;
gyro->initFn = mpu3050Init;
gyro->readFn = mpuGyroRead;
gyro->temperatureFn = mpu3050ReadTemperature;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
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;
}
acc->init = mpu6050AccInit;
acc->read = mpuAccRead;
acc->initFn = mpu6050AccInit;
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.
return true;
@ -105,9 +105,9 @@ bool mpu6050GyroDetect(gyroDev_t *gyro)
if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
return false;
}
gyro->init = mpu6050GyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady;
gyro->initFn = mpu6050GyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
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;
}
acc->init = mpu6500AccInit;
acc->read = mpuAccRead;
acc->initFn = mpu6500AccInit;
acc->readFn = mpuAccRead;
return true;
}
@ -107,9 +107,9 @@ bool mpu6500GyroDetect(gyroDev_t *gyro)
return false;
}
gyro->init = mpu6500GyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady;
gyro->initFn = mpu6500GyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;

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

@ -17,12 +17,6 @@
#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_INT_ANYRD_2CLEAR (1 << 4)
#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;
}
acc->init = mpu6000SpiAccInit;
acc->read = mpuAccRead;
acc->initFn = mpu6000SpiAccInit;
acc->readFn = mpuAccRead;
return true;
}
@ -270,9 +270,9 @@ bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady;
gyro->initFn = mpu6000SpiGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
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 MPU6000_WHO_AM_I_CONST (0x68)
// RF = Register Flag
#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;
}
acc->init = mpu6500SpiAccInit;
acc->read = mpuAccRead;
acc->initFn = mpu6500SpiAccInit;
acc->readFn = mpuAccRead;
return true;
}
@ -156,9 +156,9 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
return false;
}
gyro->init = mpu6500SpiGyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady;
gyro->initFn = mpu6500SpiGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
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;
}
acc->init = mpu9250SpiAccInit;
acc->read = mpuAccRead;
acc->initFn = mpu9250SpiAccInit;
acc->readFn = mpuAccRead;
return true;
}
@ -223,9 +223,9 @@ bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
return false;
}
gyro->init = mpu9250SpiGyroInit;
gyro->read = mpuGyroRead;
gyro->intStatus = mpuCheckDataReady;
gyro->initFn = mpu9250SpiGyroInit;
gyro->readFn = mpuGyroRead;
gyro->intStatusFn = mpuCheckDataReady;
// 16.4 dps/lsb scalefactor
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 MPU9250_WHO_AM_I_CONST (0x71)
#define MPU9255_WHO_AM_I_CONST (0x73)
#define MPU9250_BIT_RESET (0x80)
// RF = Register Flag

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

@ -26,9 +26,9 @@
bool gyroSyncCheckIntStatus(gyroDev_t *gyro)
{
if (!gyro->intStatus)
if (!gyro->intStatusFn)
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)

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

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

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

@ -292,7 +292,7 @@ bool gyroInit(void)
gyroDev0.lpf = gyroConfig()->gyro_lpf;
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->looptime, gyroConfig()->gyro_lpf, gyroConfig()->gyroSync, gyroConfig()->gyroSyncDenominator);
// driver initialisation
gyroDev0.init(&gyroDev0);
gyroDev0.initFn(&gyroDev0);
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
gyroDev0.gyroAlign = gyroConfig()->gyro_align;
@ -405,7 +405,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (gyroDev0.read(&gyroDev0)) {
if (gyroDev0.readFn(&gyroDev0)) {
if (isCalibrationComplete(&gyroCalibration)) {
// 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];
@ -447,8 +447,8 @@ void gyroUpdate(void)
void gyroReadTemperature(void)
{
if (gyroDev0.temperature) {
gyroDev0.temperature(&gyroDev0, &gyroTemperature0);
if (gyroDev0.temperatureFn) {
gyroDev0.temperatureFn(&gyroDev0, &gyroTemperature0);
}
}