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

Moved sensor global data into sensor_s structs

This commit is contained in:
Martin Budden 2016-11-19 14:11:03 +00:00
parent fd5710051e
commit b8b9c95f57
69 changed files with 359 additions and 343 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/main/blackbox/blackbox.c
View File

@ -991,11 +991,11 @@ static void loadMainState(uint32_t currentTime)
} }
for (i = 0; i < XYZ_AXIS_COUNT; i++) { for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->gyroADC[i] = lrintf(gyroADCf[i]); blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
} }
for (i = 0; i < XYZ_AXIS_COUNT; i++) { for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->accSmooth[i] = accSmooth[i]; blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
} }
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
@ -1011,12 +1011,12 @@ static void loadMainState(uint32_t currentTime)
#ifdef MAG #ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++) { for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->magADC[i] = magADC[i]; blackboxCurrent->magADC[i] = mag.magADC[i];
} }
#endif #endif
#ifdef BARO #ifdef BARO
blackboxCurrent->BaroAlt = BaroAlt; blackboxCurrent->BaroAlt = baro.BaroAlt;
#endif #endif
#ifdef SONAR #ifdef SONAR
@ -1176,8 +1176,8 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle); BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x", castFloatBytesToInt(gyro.dev.scale));
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G); BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM( BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {

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

@ -18,7 +18,6 @@
#pragma once #pragma once
#include "common/axis.h" #include "common/axis.h"
#include "common/filter.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#ifndef MPU_I2C_INSTANCE #ifndef MPU_I2C_INSTANCE
@ -34,21 +33,20 @@
#define GYRO_LPF_5HZ 6 #define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7 #define GYRO_LPF_NONE 7
typedef struct gyro_s { typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function sensorGyroInitFuncPtr init; // initialize function
sensorGyroReadFuncPtr read; // read 3 axis data function sensorGyroReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available sensorReadFuncPtr temperature; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus; sensorGyroInterruptStatusFuncPtr intStatus;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
float scale; // scalefactor float scale; // scalefactor
uint32_t targetLooptime;
uint16_t lpf;
volatile bool dataReady; volatile bool dataReady;
} gyro_t; uint16_t lpf;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
} gyroDev_t;
typedef struct acc_s { typedef struct accDev_s {
sensorAccInitFuncPtr init; // initialize function sensorAccInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr read; // read 3 axis data function
uint16_t acc_1G; uint16_t acc_1G;
char revisionCode; // a revision code for the sensor, if known char revisionCode; // a revision code for the sensor, if known
} acc_t; } accDev_t;

6
src/main/drivers/accgyro_adxl345.c
View File

@ -56,12 +56,12 @@
#define ADXL345_RANGE_16G 0x03 #define ADXL345_RANGE_16G 0x03
#define ADXL345_FIFO_STREAM 0x80 #define ADXL345_FIFO_STREAM 0x80
static void adxl345Init(acc_t *acc); static void adxl345Init(accDev_t *acc);
static bool adxl345Read(int16_t *accelData); static bool adxl345Read(int16_t *accelData);
static bool useFifo = false; static bool useFifo = false;
bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc) bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc)
{ {
uint8_t sig = 0; uint8_t sig = 0;
bool ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig); bool ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig);
@ -77,7 +77,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
return true; return true;
} }
static void adxl345Init(acc_t *acc) static void adxl345Init(accDev_t *acc)
{ {
if (useFifo) { if (useFifo) {
uint8_t fifoDepth = 16; uint8_t fifoDepth = 16;

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

@ -22,4 +22,4 @@ typedef struct drv_adxl345_config_s {
uint16_t dataRate; uint16_t dataRate;
} drv_adxl345_config_t; } drv_adxl345_config_t;
bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc); bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc);

6
src/main/drivers/accgyro_bma280.c
View File

@ -32,10 +32,10 @@
#define BMA280_PMU_BW 0x10 #define BMA280_PMU_BW 0x10
#define BMA280_PMU_RANGE 0x0F #define BMA280_PMU_RANGE 0x0F
static void bma280Init(acc_t *acc); static void bma280Init(accDev_t *acc);
static bool bma280Read(int16_t *accelData); static bool bma280Read(int16_t *accelData);
bool bma280Detect(acc_t *acc) bool bma280Detect(accDev_t *acc)
{ {
uint8_t sig = 0; uint8_t sig = 0;
bool ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig); bool ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
@ -48,7 +48,7 @@ bool bma280Detect(acc_t *acc)
return true; return true;
} }
static void bma280Init(acc_t *acc) static void bma280Init(accDev_t *acc)
{ {
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW

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

@ -17,4 +17,4 @@
#pragma once #pragma once
bool bma280Detect(acc_t *acc); bool bma280Detect(accDev_t *acc);

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

@ -54,10 +54,10 @@
#define L3G4200D_DLPF_78HZ 0x80 #define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0 #define L3G4200D_DLPF_93HZ 0xC0
static void l3g4200dInit(gyro_t *gyro); static void l3g4200dInit(gyroDev_t *gyro);
static bool l3g4200dRead(gyro_t *gyro); static bool l3g4200dRead(gyroDev_t *gyro);
bool l3g4200dDetect(gyro_t *gyro) bool l3g4200dDetect(gyroDev_t *gyro)
{ {
uint8_t deviceid; uint8_t deviceid;
@ -76,7 +76,7 @@ bool l3g4200dDetect(gyro_t *gyro)
return true; return true;
} }
static void l3g4200dInit(gyro_t *gyro) static void l3g4200dInit(gyroDev_t *gyro)
{ {
bool ack; bool ack;
@ -109,7 +109,7 @@ static void l3g4200dInit(gyro_t *gyro)
} }
// Read 3 gyro values into user-provided buffer. No overrun checking is done. // Read 3 gyro values into user-provided buffer. No overrun checking is done.
static bool l3g4200dRead(gyro_t *gyro) static bool l3g4200dRead(gyroDev_t *gyro)
{ {
uint8_t buf[6]; uint8_t buf[6];

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

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

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

@ -84,7 +84,7 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD); spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
} }
void l3gd20GyroInit(gyro_t *gyro) void l3gd20GyroInit(gyroDev_t *gyro)
{ {
UNUSED(gyro); // FIXME use it! UNUSED(gyro); // FIXME use it!
@ -120,7 +120,7 @@ void l3gd20GyroInit(gyro_t *gyro)
delay(100); delay(100);
} }
static bool l3gd20GyroRead(gyro_t *gyro) static bool l3gd20GyroRead(gyroDev_t *gyro)
{ {
uint8_t buf[6]; uint8_t buf[6];
@ -150,7 +150,7 @@ static bool l3gd20GyroRead(gyro_t *gyro)
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f #define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyro_t *gyro) bool l3gd20Detect(gyroDev_t *gyro)
{ {
gyro->init = l3gd20GyroInit; gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead; gyro->read = l3gd20GyroRead;

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

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

4
src/main/drivers/accgyro_lsm303dlhc.c
View File

@ -115,7 +115,7 @@ int32_t accelSummedSamples100Hz[3];
int32_t accelSummedSamples500Hz[3]; int32_t accelSummedSamples500Hz[3];
void lsm303dlhcAccInit(acc_t *acc) void lsm303dlhcAccInit(accDev_t *acc)
{ {
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
@ -157,7 +157,7 @@ static bool lsm303dlhcAccRead(int16_t *gyroADC)
return true; return true;
} }
bool lsm303dlhcAccDetect(acc_t *acc) bool lsm303dlhcAccDetect(accDev_t *acc)
{ {
bool ack; bool ack;
uint8_t status; uint8_t status;

24
src/main/drivers/accgyro_lsm303dlhc.h
View File

@ -195,10 +195,10 @@ typedef struct {
/** @defgroup Acc_Full_Scale_Selection /** @defgroup Acc_Full_Scale_Selection
* @{ * @{
*/ */
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< ±2 g */ #define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< +/-2 g */
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< ±4 g */ #define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< +/-4 g */
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< ±8 g */ #define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< +/-8 g */
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< ±16 g */ #define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< +/-16 g */
/** /**
* @} * @}
*/ */
@ -388,13 +388,13 @@ typedef struct {
/** @defgroup Mag_Full_Scale /** @defgroup Mag_Full_Scale
* @{ * @{
*/ */
#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = ±1.3 Gauss */ #define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = +/-1.3 Gauss */
#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = ±1.9 Gauss */ #define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = +/-1.9 Gauss */
#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = ±2.5 Gauss */ #define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = +/-2.5 Gauss */
#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = ±4.0 Gauss */ #define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = +/-4.0 Gauss */
#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = ±4.7 Gauss */ #define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = +/-4.7 Gauss */
#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = ±5.6 Gauss */ #define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = +/-5.6 Gauss */
#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = ±8.1 Gauss */ #define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = +/-8.1 Gauss */
/** /**
* @} * @}
*/ */
@ -438,5 +438,5 @@ typedef struct {
#define LSM303DLHC_TEMPSENSOR_DISABLE ((uint8_t) 0x00) /*!< Temp sensor Disable */ #define LSM303DLHC_TEMPSENSOR_DISABLE ((uint8_t) 0x00) /*!< Temp sensor Disable */
bool lsm303dlhcAccDetect(acc_t *acc); bool lsm303dlhcAccDetect(accDev_t *acc);

6
src/main/drivers/accgyro_mma845x.c
View File

@ -81,10 +81,10 @@
static uint8_t device_id; static uint8_t device_id;
static void mma8452Init(acc_t *acc); static void mma8452Init(accDev_t *acc);
static bool mma8452Read(int16_t *accelData); static bool mma8452Read(int16_t *accelData);
bool mma8452Detect(acc_t *acc) bool mma8452Detect(accDev_t *acc)
{ {
uint8_t sig = 0; uint8_t sig = 0;
bool ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig); bool ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
@ -113,7 +113,7 @@ static inline void mma8451ConfigureInterrupt(void)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
} }
static void mma8452Init(acc_t *acc) static void mma8452Init(accDev_t *acc)
{ {
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff

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

@ -17,4 +17,4 @@
#pragma once #pragma once
bool mma8452Detect(acc_t *acc); bool mma8452Detect(accDev_t *acc);

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

@ -223,7 +223,7 @@ static void mpu6050FindRevision(void)
typedef struct mpuIntRec_s { typedef struct mpuIntRec_s {
extiCallbackRec_t exti; extiCallbackRec_t exti;
gyro_t *gyro; gyroDev_t *gyro;
} mpuIntRec_t; } mpuIntRec_t;
mpuIntRec_t mpuIntRec; mpuIntRec_t mpuIntRec;
@ -235,7 +235,7 @@ mpuIntRec_t mpuIntRec;
static void mpuIntExtiHandler(extiCallbackRec_t *cb) static void mpuIntExtiHandler(extiCallbackRec_t *cb)
{ {
mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti); mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti);
gyro_t *gyro = rec->gyro; gyroDev_t *gyro = rec->gyro;
gyro->dataReady = true; gyro->dataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
@ -248,7 +248,7 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
} }
#endif #endif
static void mpuIntExtiInit(gyro_t *gyro) static void mpuIntExtiInit(gyroDev_t *gyro)
{ {
mpuIntRec.gyro = gyro; mpuIntRec.gyro = gyro;
#if defined(MPU_INT_EXTI) #if defined(MPU_INT_EXTI)
@ -313,7 +313,7 @@ bool mpuAccRead(int16_t *accData)
return true; return true;
} }
bool mpuGyroRead(gyro_t *gyro) bool mpuGyroRead(gyroDev_t *gyro)
{ {
uint8_t data[6]; uint8_t data[6];
@ -329,12 +329,12 @@ bool mpuGyroRead(gyro_t *gyro)
return true; return true;
} }
void mpuGyroInit(gyro_t *gyro) void mpuGyroInit(gyroDev_t *gyro)
{ {
mpuIntExtiInit(gyro); mpuIntExtiInit(gyro);
} }
bool checkMPUDataReady(gyro_t* gyro) bool checkMPUDataReady(gyroDev_t* gyro)
{ {
bool ret; bool ret;
if (gyro->dataReady) { if (gyro->dataReady) {

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

@ -17,7 +17,6 @@
#pragma once #pragma once
#include "io_types.h"
#include "exti.h" #include "exti.h"
// MPU6050 // MPU6050
@ -185,9 +184,9 @@ typedef struct mpuDetectionResult_s {
extern mpuDetectionResult_t mpuDetectionResult; extern mpuDetectionResult_t mpuDetectionResult;
void configureMPUDataReadyInterruptHandling(void); void configureMPUDataReadyInterruptHandling(void);
struct gyro_s; struct gyroDev_s;
void mpuGyroInit(struct gyro_s *gyro); void mpuGyroInit(struct gyroDev_s *gyro);
bool mpuAccRead(int16_t *accData); bool mpuAccRead(int16_t *accData);
bool mpuGyroRead(struct gyro_s *gyro); bool mpuGyroRead(struct gyroDev_s *gyro);
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse); mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
bool checkMPUDataReady(struct gyro_s *gyro); bool checkMPUDataReady(struct gyroDev_s *gyro);

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

@ -46,10 +46,10 @@
#define MPU3050_USER_RESET 0x01 #define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01 #define MPU3050_CLK_SEL_PLL_GX 0x01
static void mpu3050Init(gyro_t *gyro); static void mpu3050Init(gyroDev_t *gyro);
static bool mpu3050ReadTemp(int16_t *tempData); static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro) bool mpu3050Detect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_3050) { if (mpuDetectionResult.sensor != MPU_3050) {
return false; return false;
@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
return true; return true;
} }
static void mpu3050Init(gyro_t *gyro) static void mpu3050Init(gyroDev_t *gyro)
{ {
bool ack; bool ack;

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

@ -26,4 +26,4 @@
#define MPU3050_USER_CTRL 0x3D #define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E #define MPU3050_PWR_MGM 0x3E
bool mpu3050Detect(gyro_t *gyro); bool mpu3050Detect(gyroDev_t *gyro);

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

@ -50,10 +50,10 @@
#define MPU6050_SMPLRT_DIV 0 // 8000Hz #define MPU6050_SMPLRT_DIV 0 // 8000Hz
static void mpu6050AccInit(acc_t *acc); static void mpu6050AccInit(accDev_t *acc);
static void mpu6050GyroInit(gyro_t *gyro); static void mpu6050GyroInit(gyroDev_t *gyro);
bool mpu6050AccDetect(acc_t *acc) bool mpu6050AccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_60x0) { if (mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
@ -66,7 +66,7 @@ bool mpu6050AccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6050GyroDetect(gyro_t *gyro) bool mpu6050GyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0) { if (mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
@ -81,7 +81,7 @@ bool mpu6050GyroDetect(gyro_t *gyro)
return true; return true;
} }
static void mpu6050AccInit(acc_t *acc) static void mpu6050AccInit(accDev_t *acc)
{ {
switch (mpuDetectionResult.resolution) { switch (mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION: case MPU_HALF_RESOLUTION:
@ -93,7 +93,7 @@ static void mpu6050AccInit(acc_t *acc)
} }
} }
static void mpu6050GyroInit(gyro_t *gyro) static void mpu6050GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);

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

@ -17,5 +17,5 @@
#pragma once #pragma once
bool mpu6050AccDetect(acc_t *acc); bool mpu6050AccDetect(accDev_t *acc);
bool mpu6050GyroDetect(gyro_t *gyro); bool mpu6050GyroDetect(gyroDev_t *gyro);

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

@ -34,7 +34,7 @@
#include "accgyro_mpu.h" #include "accgyro_mpu.h"
#include "accgyro_mpu6500.h" #include "accgyro_mpu6500.h"
bool mpu6500AccDetect(acc_t *acc) bool mpu6500AccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false; return false;
@ -46,7 +46,7 @@ bool mpu6500AccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6500GyroDetect(gyro_t *gyro) bool mpu6500GyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false; return false;
@ -62,12 +62,12 @@ bool mpu6500GyroDetect(gyro_t *gyro)
return true; return true;
} }
void mpu6500AccInit(acc_t *acc) void mpu6500AccInit(accDev_t *acc)
{ {
acc->acc_1G = 512 * 4; acc->acc_1G = 512 * 4;
} }
void mpu6500GyroInit(gyro_t *gyro) void mpu6500GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);

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

@ -28,8 +28,8 @@
#define MPU6500_BIT_I2C_IF_DIS (1 << 4) #define MPU6500_BIT_I2C_IF_DIS (1 << 4)
#define MPU6500_BIT_RAW_RDY_EN (0x01) #define MPU6500_BIT_RAW_RDY_EN (0x01)
bool mpu6500AccDetect(acc_t *acc); bool mpu6500AccDetect(accDev_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro); bool mpu6500GyroDetect(gyroDev_t *gyro);
void mpu6500AccInit(acc_t *acc); void mpu6500AccInit(accDev_t *acc);
void mpu6500GyroInit(gyro_t *gyro); void mpu6500GyroInit(gyroDev_t *gyro);

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

@ -107,7 +107,7 @@ bool icm20689SpiDetect(void)
} }
bool icm20689SpiAccDetect(acc_t *acc) bool icm20689SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != ICM_20689_SPI) { if (mpuDetectionResult.sensor != ICM_20689_SPI) {
return false; return false;
@ -119,7 +119,7 @@ bool icm20689SpiAccDetect(acc_t *acc)
return true; return true;
} }
bool icm20689SpiGyroDetect(gyro_t *gyro) bool icm20689SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != ICM_20689_SPI) { if (mpuDetectionResult.sensor != ICM_20689_SPI) {
return false; return false;
@ -135,12 +135,12 @@ bool icm20689SpiGyroDetect(gyro_t *gyro)
return true; return true;
} }
void icm20689AccInit(acc_t *acc) void icm20689AccInit(accDev_t *acc)
{ {
acc->acc_1G = 512 * 4; acc->acc_1G = 512 * 4;
} }
void icm20689GyroInit(gyro_t *gyro) void icm20689GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);

12
src/main/drivers/accgyro_spi_icm20689.h
View File

@ -19,16 +19,16 @@
#define ICM20689_WHO_AM_I_CONST (0x98) #define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM20689_BIT_RESET (0x80) #define ICM20689_BIT_RESET (0x80)
bool icm20689AccDetect(acc_t *acc); bool icm20689AccDetect(accDev_t *acc);
bool icm20689GyroDetect(gyro_t *gyro); bool icm20689GyroDetect(gyroDev_t *gyro);
void icm20689AccInit(acc_t *acc); void icm20689AccInit(accDev_t *acc);
void icm20689GyroInit(gyro_t *gyro); void icm20689GyroInit(gyroDev_t *gyro);
bool icm20689SpiDetect(void); bool icm20689SpiDetect(void);
bool icm20689SpiAccDetect(acc_t *acc); bool icm20689SpiAccDetect(accDev_t *acc);
bool icm20689SpiGyroDetect(gyro_t *gyro); bool icm20689SpiGyroDetect(gyroDev_t *gyro);
bool icm20689WriteRegister(uint8_t reg, uint8_t data); bool icm20689WriteRegister(uint8_t reg, uint8_t data);
bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

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

@ -124,7 +124,7 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true; return true;
} }
void mpu6000SpiGyroInit(gyro_t *gyro) void mpu6000SpiGyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
@ -145,7 +145,7 @@ void mpu6000SpiGyroInit(gyro_t *gyro)
} }
} }
void mpu6000SpiAccInit(acc_t *acc) void mpu6000SpiAccInit(accDev_t *acc)
{ {
acc->acc_1G = 512 * 4; acc->acc_1G = 512 * 4;
} }
@ -254,7 +254,7 @@ static void mpu6000AccAndGyroInit(void)
mpuSpi6000InitDone = true; mpuSpi6000InitDone = true;
} }
bool mpu6000SpiAccDetect(acc_t *acc) bool mpu6000SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) { if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false; return false;
@ -266,7 +266,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6000SpiGyroDetect(gyro_t *gyro) bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) { if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false; return false;

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

@ -17,8 +17,8 @@
bool mpu6000SpiDetect(void); bool mpu6000SpiDetect(void);
bool mpu6000SpiAccDetect(acc_t *acc); bool mpu6000SpiAccDetect(accDev_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro); bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
bool mpu6000WriteRegister(uint8_t reg, uint8_t data); bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

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

@ -94,12 +94,12 @@ bool mpu6500SpiDetect(void)
return false; return false;
} }
void mpu6500SpiAccInit(acc_t *acc) void mpu6500SpiAccInit(accDev_t *acc)
{ {
mpu6500AccInit(acc); mpu6500AccInit(acc);
} }
void mpu6500SpiGyroInit(gyro_t *gyro) void mpu6500SpiGyroInit(gyroDev_t *gyro)
{ {
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW); spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
delayMicroseconds(1); delayMicroseconds(1);
@ -114,7 +114,7 @@ void mpu6500SpiGyroInit(gyro_t *gyro)
delayMicroseconds(1); delayMicroseconds(1);
} }
bool mpu6500SpiAccDetect(acc_t *acc) bool mpu6500SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) { if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false; return false;
@ -126,7 +126,7 @@ bool mpu6500SpiAccDetect(acc_t *acc)
return true; return true;
} }
bool mpu6500SpiGyroDetect(gyro_t *gyro) bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) { if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false; return false;

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

@ -19,11 +19,11 @@
bool mpu6500SpiDetect(void); bool mpu6500SpiDetect(void);
void mpu6500SpiAccInit(acc_t *acc); void mpu6500SpiAccInit(accDev_t *acc);
void mpu6500SpiGyroInit(gyro_t *gyro); void mpu6500SpiGyroInit(gyroDev_t *gyro);
bool mpu6500SpiAccDetect(acc_t *acc); bool mpu6500SpiAccDetect(accDev_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro); bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
bool mpu6500WriteRegister(uint8_t reg, uint8_t data); bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

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

@ -96,7 +96,7 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true; return true;
} }
void mpu9250SpiGyroInit(gyro_t *gyro) void mpu9250SpiGyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
@ -114,7 +114,7 @@ void mpu9250SpiGyroInit(gyro_t *gyro)
} }
} }
void mpu9250SpiAccInit(acc_t *acc) void mpu9250SpiAccInit(accDev_t *acc)
{ {
acc->acc_1G = 512 * 8; acc->acc_1G = 512 * 8;
} }
@ -209,7 +209,7 @@ bool mpu9250SpiDetect(void)
return true; return true;
} }
bool mpu9250SpiAccDetect(acc_t *acc) bool mpu9250SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_9250_SPI) { if (mpuDetectionResult.sensor != MPU_9250_SPI) {
return false; return false;
@ -221,7 +221,7 @@ bool mpu9250SpiAccDetect(acc_t *acc)
return true; return true;
} }
bool mpu9250SpiGyroDetect(gyro_t *gyro) bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_9250_SPI) { if (mpuDetectionResult.sensor != MPU_9250_SPI) {
return false; return false;

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

@ -27,8 +27,8 @@ void mpu9250ResetGyro(void);
bool mpu9250SpiDetect(void); bool mpu9250SpiDetect(void);
bool mpu9250SpiAccDetect(acc_t *acc); bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyro_t *gyro); bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250WriteRegister(uint8_t reg, uint8_t data); bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data); bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);

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

@ -20,7 +20,7 @@
typedef void (*baroOpFuncPtr)(void); // baro start operation typedef void (*baroOpFuncPtr)(void); // baro start operation
typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef struct baro_s { typedef struct baroDev_s {
uint16_t ut_delay; uint16_t ut_delay;
uint16_t up_delay; uint16_t up_delay;
baroOpFuncPtr start_ut; baroOpFuncPtr start_ut;
@ -28,7 +28,7 @@ typedef struct baro_s {
baroOpFuncPtr start_up; baroOpFuncPtr start_up;
baroOpFuncPtr get_up; baroOpFuncPtr get_up;
baroCalculateFuncPtr calculate; baroCalculateFuncPtr calculate;
} baro_t; } baroDev_t;
#ifndef BARO_I2C_INSTANCE #ifndef BARO_I2C_INSTANCE
#define BARO_I2C_INSTANCE I2C_DEVICE #define BARO_I2C_INSTANCE I2C_DEVICE

2
src/main/drivers/barometer_bmp085.c
View File

@ -154,7 +154,7 @@ void bmp085Disable(const bmp085Config_t *config)
BMP085_OFF; BMP085_OFF;
} }
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro) bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
{ {
uint8_t data; uint8_t data;
bool ack; bool ack;

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

@ -22,7 +22,7 @@ typedef struct bmp085Config_s {
ioTag_t eocIO; ioTag_t eocIO;
} bmp085Config_t; } bmp085Config_t;
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro); bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro);
void bmp085Disable(const bmp085Config_t *config); void bmp085Disable(const bmp085Config_t *config);
#if defined(BARO_EOC_GPIO) #if defined(BARO_EOC_GPIO)

2
src/main/drivers/barometer_bmp280.c
View File

@ -65,7 +65,7 @@ static void bmp280_get_up(void);
#endif #endif
STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature); STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
bool bmp280Detect(baro_t *baro) bool bmp280Detect(baroDev_t *baro)
{ {
if (bmp280InitDone) if (bmp280InitDone)
return true; return true;

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

@ -56,5 +56,5 @@
#define T_SETUP_PRESSURE_MAX (10) #define T_SETUP_PRESSURE_MAX (10)
// 10/16 = 0.625 ms // 10/16 = 0.625 ms
bool bmp280Detect(baro_t *baro); bool bmp280Detect(baroDev_t *baro);

2
src/main/drivers/barometer_ms5611.c
View File

@ -59,7 +59,7 @@ STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement
STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM
static uint8_t ms5611_osr = CMD_ADC_4096; static uint8_t ms5611_osr = CMD_ADC_4096;
bool ms5611Detect(baro_t *baro) bool ms5611Detect(baroDev_t *baro)
{ {
uint8_t sig; uint8_t sig;
int i; int i;

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

@ -17,4 +17,4 @@
#pragma once #pragma once
bool ms5611Detect(baro_t *baro); bool ms5611Detect(baroDev_t *baro);

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

@ -19,10 +19,10 @@
#include "sensor.h" #include "sensor.h"
typedef struct mag_s { typedef struct magDev_s {
sensorInitFuncPtr init; // initialize function sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr read; // read 3 axis data function
} mag_t; } magDev_t;
#ifndef MAG_I2C_INSTANCE #ifndef MAG_I2C_INSTANCE
#define MAG_I2C_INSTANCE I2C_DEVICE #define MAG_I2C_INSTANCE I2C_DEVICE

2
src/main/drivers/compass_ak8963.c
View File

@ -188,7 +188,7 @@ bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
} }
#endif #endif
bool ak8963Detect(mag_t *mag) bool ak8963Detect(magDev_t *mag)
{ {
uint8_t sig = 0; uint8_t sig = 0;

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

@ -17,6 +17,6 @@
#pragma once #pragma once
bool ak8963Detect(mag_t *mag); bool ak8963Detect(magDev_t *mag);
void ak8963Init(void); void ak8963Init(void);
bool ak8963Read(int16_t *magData); bool ak8963Read(int16_t *magData);

2
src/main/drivers/compass_ak8975.c
View File

@ -57,7 +57,7 @@
#define AK8975_MAG_REG_CNTL 0x0a #define AK8975_MAG_REG_CNTL 0x0a
#define AK8975_MAG_REG_ASCT 0x0c // self test #define AK8975_MAG_REG_ASCT 0x0c // self test
bool ak8975Detect(mag_t *mag) bool ak8975Detect(magDev_t *mag)
{ {
uint8_t sig = 0; uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig); bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);

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

@ -17,6 +17,6 @@
#pragma once #pragma once
bool ak8975Detect(mag_t *mag); bool ak8975Detect(magDev_t *mag);
void ak8975Init(void); void ak8975Init(void);
bool ak8975Read(int16_t *magData); bool ak8975Read(int16_t *magData);

2
src/main/drivers/compass_hmc5883l.c
View File

@ -167,7 +167,7 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)
#endif #endif
} }
bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse) bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
{ {
hmc5883Config = hmc5883ConfigToUse; hmc5883Config = hmc5883ConfigToUse;

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

@ -23,6 +23,6 @@ typedef struct hmc5883Config_s {
ioTag_t intTag; ioTag_t intTag;
} hmc5883Config_t; } hmc5883Config_t;
bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse); bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
void hmc5883lInit(void); void hmc5883lInit(void);
bool hmc5883lRead(int16_t *magData); bool hmc5883lRead(int16_t *magData);

2
src/main/drivers/gyro_sync.c
View File

@ -16,7 +16,7 @@
static uint8_t mpuDividerDrops; static uint8_t mpuDividerDrops;
bool gyroSyncCheckUpdate(gyro_t *gyro) bool gyroSyncCheckUpdate(gyroDev_t *gyro)
{ {
if (!gyro->intStatus) if (!gyro->intStatus)
return false; return false;

5
src/main/drivers/gyro_sync.h
View File

@ -5,7 +5,8 @@
* Author: borisb * Author: borisb
*/ */
struct gyro_s; struct gyroDev_s;
bool gyroSyncCheckUpdate(struct gyro_s *gyro);
bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(void); uint8_t gyroMPU6xxxGetDividerDrops(void);
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

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

@ -17,12 +17,12 @@
#pragma once #pragma once
struct acc_s; struct accDev_s;
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc); // sensor init prototype
struct gyro_s; struct gyroDev_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyro_s *gyro); typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyro_s *gyro); typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyro_s *gyro); typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorInterruptFuncPtr)(void); typedef bool (*sensorInterruptFuncPtr)(void);

8
src/main/fc/fc_msp.c
View File

@ -607,15 +607,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RAW_IMU: case MSP_RAW_IMU:
{ {
// Hack scale due to choice of units for sensor data in multiwii // Hack scale due to choice of units for sensor data in multiwii
const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1; const uint8_t scale = (acc.dev.acc_1G > 512) ? 4 : 1;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, accSmooth[i] / scale); sbufWriteU16(dst, acc.accSmooth[i] / scale);
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, lrintf(gyroADCf[i] / gyro.scale)); sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, magADC[i]); sbufWriteU16(dst, mag.magADC[i]);
} }
} }
break; break;

2
src/main/fc/fc_tasks.c
View File

@ -219,7 +219,7 @@ void fcTasksInit(void)
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true); setTaskEnabled(TASK_ACCEL, true);
rescheduleTask(TASK_ACCEL, accSamplingInterval); rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
} }
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));

4
src/main/fc/mw.c
View File

@ -704,8 +704,8 @@ void subTaskMainSubprocesses(void)
const uint32_t startTime = micros(); const uint32_t startTime = micros();
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature) { if (gyro.dev.temperature) {
gyro.temperature(&telemTemperature1); gyro.dev.temperature(&telemTemperature1);
} }
#ifdef MAG #ifdef MAG

20
src/main/flight/altitudehold.c
View File

@ -237,9 +237,9 @@ void calculateEstimatedAltitude(uint32_t currentTime)
accAlt = 0; accAlt = 0;
} }
BaroAlt = baroCalculateAltitude(); baro.BaroAlt = baroCalculateAltitude();
#else #else
BaroAlt = 0; baro.BaroAlt = 0;
#endif #endif
#ifdef SONAR #ifdef SONAR
@ -248,14 +248,14 @@ void calculateEstimatedAltitude(uint32_t currentTime)
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) { if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// just use the SONAR // just use the SONAR
baroAlt_offset = BaroAlt - sonarAlt; baroAlt_offset = baro.BaroAlt - sonarAlt;
BaroAlt = sonarAlt; baro.BaroAlt = sonarAlt;
} else { } else {
BaroAlt -= baroAlt_offset; baro.BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) { if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter // SONAR in range, so use complementary filter
sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm); sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition); baro.BaroAlt = sonarAlt * sonarTransition + baro.BaroAlt * (1.0f - sonarTransition);
} }
} }
#endif #endif
@ -272,7 +272,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
// Integrator - Altitude in cm // Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc; vel += vel_acc;
#ifdef DEBUG_ALT_HOLD #ifdef DEBUG_ALT_HOLD
@ -292,7 +292,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
#ifdef SONAR #ifdef SONAR
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) { if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// the sonar has the best range // the sonar has the best range
EstAlt = BaroAlt; EstAlt = baro.BaroAlt;
} else { } else {
EstAlt = accAlt; EstAlt = accAlt;
} }
@ -300,8 +300,8 @@ void calculateEstimatedAltitude(uint32_t currentTime)
EstAlt = accAlt; EstAlt = accAlt;
#endif #endif
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime; baroVel = (baro.BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt; lastBaroAlt = baro.BaroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero

28
src/main/flight/imu.c
View File

@ -120,7 +120,7 @@ void imuConfigure(
void imuInit(void) void imuInit(void)
{ {
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle)); smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
accVelScale = 9.80665f / acc.acc_1G / 10000.0f; accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
imuComputeRotationMatrix(); imuComputeRotationMatrix();
} }
@ -172,9 +172,9 @@ void imuCalculateAcceleration(uint32_t deltaT)
// deltaT is measured in us ticks // deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f; dT = (float)deltaT * 1e-6f;
accel_ned.V.X = accSmooth[0]; accel_ned.V.X = acc.accSmooth[X];
accel_ned.V.Y = accSmooth[1]; accel_ned.V.Y = acc.accSmooth[Y];
accel_ned.V.Z = accSmooth[2]; accel_ned.V.Z = acc.accSmooth[Z];
imuTransformVectorBodyToEarth(&accel_ned); imuTransformVectorBodyToEarth(&accel_ned);
@ -185,7 +185,7 @@ void imuCalculateAcceleration(uint32_t deltaT)
} }
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else } else
accel_ned.V.Z -= acc.acc_1G; accel_ned.V.Z -= acc.dev.acc_1G;
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
@ -355,10 +355,10 @@ static bool imuIsAccelerometerHealthy(void)
int32_t accMagnitude = 0; int32_t accMagnitude = 0;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis]; accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
} }
accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.acc_1G)); accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G));
// Accept accel readings only in range 0.90g - 1.10g // Accept accel readings only in range 0.90g - 1.10g
return (81 < accMagnitude) && (accMagnitude < 121); return (81 < accMagnitude) && (accMagnitude < 121);
@ -366,7 +366,7 @@ static bool imuIsAccelerometerHealthy(void)
static bool isMagnetometerHealthy(void) static bool isMagnetometerHealthy(void)
{ {
return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0); return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
} }
static void imuCalculateEstimatedAttitude(uint32_t currentTime) static void imuCalculateEstimatedAttitude(uint32_t currentTime)
@ -396,9 +396,9 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
#endif #endif
imuMahonyAHRSupdate(deltaT * 1e-6f, imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroADCf[X]), DEGREES_TO_RADIANS(gyroADCf[Y]), DEGREES_TO_RADIANS(gyroADCf[Z]), DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z], useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
useMag, magADC[X], magADC[Y], magADC[Z], useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
useYaw, rawYawError); useYaw, rawYawError);
imuUpdateEulerAngles(); imuUpdateEulerAngles();
@ -419,9 +419,9 @@ void imuUpdateAttitude(uint32_t currentTime)
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude(currentTime); imuCalculateEstimatedAttitude(currentTime);
} else { } else {
accSmooth[X] = 0; acc.accSmooth[X] = 0;
accSmooth[Y] = 0; acc.accSmooth[Y] = 0;
accSmooth[Z] = 0; acc.accSmooth[Z] = 0;
} }
} }

2
src/main/flight/pid.c
View File

@ -234,7 +234,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
} }
} }
const float PVRate = gyroADCf[axis]; // Process variable from gyro output in deg/sec const float PVRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
// --------low-level gyro-based PID based on 2DOF PID controller. ---------- // --------low-level gyro-based PID based on 2DOF PID controller. ----------
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------

6
src/main/io/dashboard.c
View File

@ -489,14 +489,14 @@ void showSensorsPage(void)
i2c_OLED_send_string(" X Y Z"); i2c_OLED_send_string(" X Y Z");
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]); tfp_sprintf(lineBuffer, format, "ACC", acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z]);
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
} }
if (sensors(SENSOR_GYRO)) { if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyroADCf[X]), lrintf(gyroADCf[Y]), lrintf(gyroADCf[Z])); tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
@ -504,7 +504,7 @@ void showSensorsPage(void)
#ifdef MAG #ifdef MAG
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]); tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]);
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);

8
src/main/io/osd.c
View File

@ -190,7 +190,7 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_ALTITUDE: case OSD_ALTITUDE:
{ {
int32_t alt = osdGetAltitude(BaroAlt); int32_t alt = osdGetAltitude(baro.BaroAlt);
sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
break; break;
} }
@ -451,7 +451,7 @@ void osdUpdateAlarms(void)
// This is overdone? // This is overdone?
// uint16_t *itemPos = osdProfile()->item_pos; // uint16_t *itemPos = osdProfile()->item_pos;
int32_t alt = osdGetAltitude(BaroAlt) / 100; int32_t alt = osdGetAltitude(baro.BaroAlt) / 100;
statRssi = rssi * 100 / 1024; statRssi = rssi * 100 / 1024;
if (statRssi < pOsdProfile->rssi_alarm) if (statRssi < pOsdProfile->rssi_alarm)
@ -525,8 +525,8 @@ static void osdUpdateStats(void)
if (stats.min_rssi > statRssi) if (stats.min_rssi > statRssi)
stats.min_rssi = statRssi; stats.min_rssi = statRssi;
if (stats.max_altitude < BaroAlt) if (stats.max_altitude < baro.BaroAlt)
stats.max_altitude = BaroAlt; stats.max_altitude = baro.BaroAlt;
} }
static void osdShowStats(void) static void osdShowStats(void)

4
src/main/io/serial_cli.c
View File

@ -3586,8 +3586,8 @@ static void cliStatus(char *cmdline)
cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware); cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware);
if (mask == SENSOR_ACC && acc.revisionCode) { if (mask == SENSOR_ACC && acc.dev.revisionCode) {
cliPrintf(".%c", acc.revisionCode); cliPrintf(".%c", acc.dev.revisionCode);
} }
} }
} }

2
src/main/rx/jetiexbus.c
View File

@ -528,7 +528,7 @@ void handleJetiExBusTelemetry(void)
if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
jetiExSensors[EX_VOLTAGE].value = vbat; jetiExSensors[EX_VOLTAGE].value = vbat;
jetiExSensors[EX_CURRENT].value = amperage; jetiExSensors[EX_CURRENT].value = amperage;
jetiExSensors[EX_ALTITUDE].value = BaroAlt; jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;
jetiExSensors[EX_CAPACITY].value = mAhDrawn; jetiExSensors[EX_CAPACITY].value = mAhDrawn;
jetiExSensors[EX_FRAMES_LOST].value = framesLost; jetiExSensors[EX_FRAMES_LOST].value = framesLost;
jetiExSensors[EX_TIME_DIFF].value = timeDiff; jetiExSensors[EX_TIME_DIFF].value = timeDiff;

42
src/main/sensors/acceleration.c
View File

@ -39,11 +39,7 @@
#include "config/feature.h" #include "config/feature.h"
int32_t accSmooth[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions acc_t acc; // acc access functions
sensor_align_e accAlign = 0;
uint32_t accSamplingInterval;
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
@ -65,19 +61,19 @@ void accInit(uint32_t gyroSamplingInverval)
case 375: case 375:
case 250: case 250:
case 125: case 125:
accSamplingInterval = 1000; acc.accSamplingInterval = 1000;
break; break;
case 1000: case 1000:
default: default:
#ifdef STM32F10X #ifdef STM32F10X
accSamplingInterval = 1000; acc.accSamplingInterval = 1000;
#else #else
accSamplingInterval = 1000; acc.accSamplingInterval = 1000;
#endif #endif
} }
if (accLpfCutHz) { if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval); biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
} }
} }
} }
@ -119,10 +115,10 @@ static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims
a[axis] = 0; a[axis] = 0;
// Sum up CALIBRATING_ACC_CYCLES readings // Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accSmooth[axis]; a[axis] += acc.accSmooth[axis];
// Reset global variables to prevent other code from using un-calibrated data // Reset global variables to prevent other code from using un-calibrated data
accSmooth[axis] = 0; acc.accSmooth[axis] = 0;
accelerationTrims->raw[axis] = 0; accelerationTrims->raw[axis] = 0;
} }
@ -130,7 +126,7 @@ static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.acc_1G; accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.dev.acc_1G;
resetRollAndPitchTrims(rollAndPitchTrims); resetRollAndPitchTrims(rollAndPitchTrims);
@ -161,9 +157,9 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP
if (InflightcalibratingA == 50) if (InflightcalibratingA == 50)
b[axis] = 0; b[axis] = 0;
// Sum up 50 readings // Sum up 50 readings
b[axis] += accSmooth[axis]; b[axis] += acc.accSmooth[axis];
// Clear global variables for next reading // Clear global variables for next reading
accSmooth[axis] = 0; acc.accSmooth[axis] = 0;
accelerationTrims->raw[axis] = 0; accelerationTrims->raw[axis] = 0;
} }
// all values are measured // all values are measured
@ -185,7 +181,7 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP
AccInflightCalibrationSavetoEEProm = false; AccInflightCalibrationSavetoEEProm = false;
accelerationTrims->raw[X] = b[X] / 50; accelerationTrims->raw[X] = b[X] / 50;
accelerationTrims->raw[Y] = b[Y] / 50; accelerationTrims->raw[Y] = b[Y] / 50;
accelerationTrims->raw[Z] = b[Z] / 50 - acc.acc_1G; // for nunchuck 200=1G accelerationTrims->raw[Z] = b[Z] / 50 - acc.dev.acc_1G; // for nunchuck 200=1G
resetRollAndPitchTrims(rollAndPitchTrims); resetRollAndPitchTrims(rollAndPitchTrims);
@ -195,31 +191,31 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP
static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims) static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
{ {
accSmooth[X] -= accelerationTrims->raw[X]; acc.accSmooth[X] -= accelerationTrims->raw[X];
accSmooth[Y] -= accelerationTrims->raw[Y]; acc.accSmooth[Y] -= accelerationTrims->raw[Y];
accSmooth[Z] -= accelerationTrims->raw[Z]; acc.accSmooth[Z] -= accelerationTrims->raw[Z];
} }
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{ {
int16_t accADCRaw[XYZ_AXIS_COUNT]; int16_t accADCRaw[XYZ_AXIS_COUNT];
if (!acc.read(accADCRaw)) { if (!acc.dev.read(accADCRaw)) {
return; return;
} }
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis]; if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
accSmooth[axis] = accADCRaw[axis]; acc.accSmooth[axis] = accADCRaw[axis];
} }
if (accLpfCutHz) { if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); acc.accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accSmooth[axis]));
} }
} }
alignSensors(accSmooth, accAlign); alignSensors(acc.accSmooth, acc.accAlign);
if (!isAccelerationCalibrationComplete()) { if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims); performAcclerationCalibration(rollAndPitchTrims);
@ -240,9 +236,9 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
void setAccelerationFilter(uint16_t initialAccLpfCutHz) void setAccelerationFilter(uint16_t initialAccLpfCutHz)
{ {
accLpfCutHz = initialAccLpfCutHz; accLpfCutHz = initialAccLpfCutHz;
if (accSamplingInterval) { if (acc.accSamplingInterval) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval); biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
} }
} }
} }

11
src/main/sensors/acceleration.h
View File

@ -36,11 +36,14 @@ typedef enum {
ACC_MAX = ACC_FAKE ACC_MAX = ACC_FAKE
} accelerationSensor_e; } accelerationSensor_e;
extern sensor_align_e accAlign; typedef struct acc_s {
extern acc_t acc; accDev_t dev;
extern uint32_t accSamplingInterval; sensor_align_e accAlign;
uint32_t accSamplingInterval;
int32_t accSmooth[XYZ_AXIS_COUNT];
} acc_t;
extern int32_t accSmooth[XYZ_AXIS_COUNT]; extern acc_t acc;
typedef struct rollAndPitchTrims_s { typedef struct rollAndPitchTrims_s {
int16_t roll; int16_t roll;

30
src/main/sensors/barometer.c
View File

@ -21,9 +21,6 @@
#include "platform.h" #include "platform.h"
int32_t BaroAlt = 0;
#ifdef BARO
#include "common/maths.h" #include "common/maths.h"
#include "drivers/barometer.h" #include "drivers/barometer.h"
@ -32,9 +29,12 @@ int32_t BaroAlt = 0;
#include "sensors/barometer.h" #include "sensors/barometer.h"
baro_t baro; // barometer access functions baro_t baro; // barometer access functions
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
int32_t baroPressure = 0; #ifdef BARO
int32_t baroTemperature = 0;
static uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
static int32_t baroPressure = 0;
static int32_t baroTemperature = 0;
static int32_t baroGroundAltitude = 0; static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0; static int32_t baroGroundPressure = 0;
@ -126,19 +126,19 @@ uint32_t baroUpdate(void)
switch (state) { switch (state) {
default: default:
case BAROMETER_NEEDS_SAMPLES: case BAROMETER_NEEDS_SAMPLES:
baro.get_ut(); baro.dev.get_ut();
baro.start_up(); baro.dev.start_up();
state = BAROMETER_NEEDS_CALCULATION; state = BAROMETER_NEEDS_CALCULATION;
return baro.up_delay; return baro.dev.up_delay;
break; break;
case BAROMETER_NEEDS_CALCULATION: case BAROMETER_NEEDS_CALCULATION:
baro.get_up(); baro.dev.get_up();
baro.start_ut(); baro.dev.start_ut();
baro.calculate(&baroPressure, &baroTemperature); baro.dev.calculate(&baroPressure, &baroTemperature);
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure); baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_SAMPLES; state = BAROMETER_NEEDS_SAMPLES;
return baro.ut_delay; return baro.dev.ut_delay;
break; break;
} }
} }
@ -151,9 +151,9 @@ int32_t baroCalculateAltitude(void)
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp -= baroGroundAltitude; BaroAlt_tmp -= baroGroundAltitude;
BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
return BaroAlt; return baro.BaroAlt;
} }
void performBaroCalibrationCycle(void) void performBaroCalibrationCycle(void)

11
src/main/sensors/barometer.h
View File

@ -17,6 +17,8 @@
#pragma once #pragma once
#include "drivers/barometer.h"
typedef enum { typedef enum {
BARO_DEFAULT = 0, BARO_DEFAULT = 0,
BARO_NONE = 1, BARO_NONE = 1,
@ -35,8 +37,13 @@ typedef struct barometerConfig_s {
float baro_cf_alt; // apply CF to use ACC for height estimation float baro_cf_alt; // apply CF to use ACC for height estimation
} barometerConfig_t; } barometerConfig_t;
extern int32_t BaroAlt; typedef struct baro_s {
extern int32_t baroTemperature; // Use temperature for telemetry baroDev_t dev;
int32_t BaroAlt;
int32_t baroTemperature; // Use temperature for telemetry
} baro_t;
extern baro_t baro;
void useBarometerConfig(barometerConfig_t *barometerConfigToUse); void useBarometerConfig(barometerConfig_t *barometerConfigToUse);
bool isBaroCalibrationComplete(void); bool isBaroCalibrationComplete(void);

28
src/main/sensors/compass.c
View File

@ -36,8 +36,6 @@
#endif #endif
mag_t mag; // mag access functions mag_t mag; // mag access functions
int32_t magADC[XYZ_AXIS_COUNT];
sensor_align_e magAlign = 0;
#ifdef MAG #ifdef MAG
@ -48,7 +46,7 @@ void compassInit(void)
{ {
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
LED1_ON; LED1_ON;
mag.init(); mag.dev.init();
LED1_OFF; LED1_OFF;
magInit = 1; magInit = 1;
} }
@ -60,34 +58,34 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
static flightDynamicsTrims_t magZeroTempMax; static flightDynamicsTrims_t magZeroTempMax;
uint32_t axis; uint32_t axis;
mag.read(magADCRaw); mag.dev.read(magADCRaw);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; // int32_t copy to work with for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) mag.magADC[axis] = magADCRaw[axis]; // int32_t copy to work with
alignSensors(magADC, magAlign); alignSensors(mag.magADC, mag.magAlign);
if (STATE(CALIBRATE_MAG)) { if (STATE(CALIBRATE_MAG)) {
tCal = currentTime; tCal = currentTime;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0; magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = magADC[axis]; magZeroTempMin.raw[axis] = mag.magADC[axis];
magZeroTempMax.raw[axis] = magADC[axis]; magZeroTempMax.raw[axis] = mag.magADC[axis];
} }
DISABLE_STATE(CALIBRATE_MAG); DISABLE_STATE(CALIBRATE_MAG);
} }
if (magInit) { // we apply offset only once mag calibration is done if (magInit) { // we apply offset only once mag calibration is done
magADC[X] -= magZero->raw[X]; mag.magADC[X] -= magZero->raw[X];
magADC[Y] -= magZero->raw[Y]; mag.magADC[Y] -= magZero->raw[Y];
magADC[Z] -= magZero->raw[Z]; mag.magADC[Z] -= magZero->raw[Z];
} }
if (tCal != 0) { if (tCal != 0) {
if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE; LED0_TOGGLE;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
if (magADC[axis] < magZeroTempMin.raw[axis]) if (mag.magADC[axis] < magZeroTempMin.raw[axis])
magZeroTempMin.raw[axis] = magADC[axis]; magZeroTempMin.raw[axis] = mag.magADC[axis];
if (magADC[axis] > magZeroTempMax.raw[axis]) if (mag.magADC[axis] > magZeroTempMax.raw[axis])
magZeroTempMax.raw[axis] = magADC[axis]; magZeroTempMax.raw[axis] = mag.magADC[axis];
} }
} else { } else {
tCal = 0; tCal = 0;

15
src/main/sensors/compass.h
View File

@ -19,6 +19,8 @@
#include "drivers/compass.h" #include "drivers/compass.h"
#include "sensors/sensors.h"
// Type of magnetometer used/detected // Type of magnetometer used/detected
typedef enum { typedef enum {
@ -30,6 +32,15 @@ typedef enum {
MAG_MAX = MAG_AK8963 MAG_MAX = MAG_AK8963
} magSensor_e; } magSensor_e;
typedef struct mag_s {
magDev_t dev;
sensor_align_e magAlign;
int32_t magADC[XYZ_AXIS_COUNT];
float magneticDeclination;
} mag_t;
extern mag_t mag;
typedef struct compassConfig_s { typedef struct compassConfig_s {
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
@ -39,7 +50,3 @@ void compassInit(void);
union flightDynamicsTrims_u; union flightDynamicsTrims_u;
void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero); void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);
extern int32_t magADC[XYZ_AXIS_COUNT];
extern sensor_align_e magAlign;
extern mag_t mag;

31
src/main/sensors/gyro.c
View File

@ -37,7 +37,6 @@
#include "sensors/gyro.h" #include "sensors/gyro.h"
gyro_t gyro; // gyro access functions gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
static int32_t gyroADC[XYZ_AXIS_COUNT]; static int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT];
@ -175,15 +174,15 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
void gyroUpdate(void) void gyroUpdate(void)
{ {
// range: +/- 8192; +/- 2000 deg/sec // range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(&gyro)) { if (!gyro.dev.read(&gyro.dev)) {
return; return;
} }
gyro.dataReady = false; gyro.dev.dataReady = false;
gyroADC[X] = gyro.gyroADCRaw[X]; gyroADC[X] = gyro.dev.gyroADCRaw[X];
gyroADC[Y] = gyro.gyroADCRaw[Y]; gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyroADC[Z] = gyro.gyroADCRaw[Z]; gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
alignSensors(gyroADC, gyroAlign); alignSensors(gyroADC, gyro.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete(); const bool calibrationComplete = isGyroCalibrationComplete();
if (!calibrationComplete) { if (!calibrationComplete) {
@ -193,20 +192,24 @@ void gyroUpdate(void)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis]; gyroADC[axis] -= gyroZero[axis];
// scale gyro output to degrees per second // scale gyro output to degrees per second
gyroADCf[axis] = (float)gyroADC[axis] * gyro.scale; gyroADCf[axis] = (float)gyroADC[axis] * gyro.dev.scale;
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf[axis])); gyroADC[axis] -= gyroZero[axis];
// scale gyro output to degrees per second
gyro.gyroADCf[axis] = (float)gyroADC[axis] * gyro.dev.scale;
gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf[axis]); DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyro.gyroADCf[axis]));
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf[axis])); gyro.gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyro.gyroADCf[axis]);
gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]); DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyro.gyroADCf[axis]));
gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]); gyro.gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyro.gyroADCf[axis]);
gyro.gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyro.gyroADCf[axis]);
if (!calibrationComplete) { if (!calibrationComplete) {
gyroADC[axis] = lrintf(gyroADCf[axis] / gyro.scale); gyroADC[axis] = lrintf(gyro.gyroADCf[axis] / gyro.dev.scale);
} }
} }
} }

9
src/main/sensors/gyro.h
View File

@ -35,9 +35,14 @@ typedef enum {
GYRO_MAX = GYRO_FAKE GYRO_MAX = GYRO_FAKE
} gyroSensor_e; } gyroSensor_e;
extern gyro_t gyro; typedef struct gyro_s {
gyroDev_t dev;
uint32_t targetLooptime;
sensor_align_e gyroAlign;
float gyroADCf[XYZ_AXIS_COUNT];
} gyro_t;
extern float gyroADCf[XYZ_AXIS_COUNT]; extern gyro_t gyro;
typedef struct gyroConfig_s { typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.

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

@ -78,12 +78,6 @@
#include "hardware_revision.h" #include "hardware_revision.h"
#endif #endif
extern float magneticDeclination;
extern gyro_t gyro;
extern baro_t baro;
extern acc_t acc;
extern sensor_align_e gyroAlign;
uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
@ -102,12 +96,12 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
#ifdef USE_FAKE_GYRO #ifdef USE_FAKE_GYRO
int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 }; int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 };
static void fakeGyroInit(gyro_t *gyro) static void fakeGyroInit(gyroDev_t *gyro)
{ {
UNUSED(gyro); UNUSED(gyro);
} }
static bool fakeGyroRead(gyro_t *gyro) static bool fakeGyroRead(gyroDev_t *gyro)
{ {
for (int i = 0; i < XYZ_AXIS_COUNT; ++i) { for (int i = 0; i < XYZ_AXIS_COUNT; ++i) {
gyro->gyroADCRaw[X] = fake_gyro_values[i]; gyro->gyroADCRaw[X] = fake_gyro_values[i];
@ -123,13 +117,13 @@ static bool fakeGyroReadTemp(int16_t *tempData)
} }
static bool fakeGyroInitStatus(gyro_t *gyro) static bool fakeGyroInitStatus(gyroDev_t *gyro)
{ {
UNUSED(gyro); UNUSED(gyro);
return true; return true;
} }
bool fakeGyroDetect(gyro_t *gyro) bool fakeGyroDetect(gyroDev_t *gyro)
{ {
gyro->init = fakeGyroInit; gyro->init = fakeGyroInit;
gyro->intStatus = fakeGyroInitStatus; gyro->intStatus = fakeGyroInitStatus;
@ -142,7 +136,9 @@ bool fakeGyroDetect(gyro_t *gyro)
#ifdef USE_FAKE_ACC #ifdef USE_FAKE_ACC
int16_t fake_acc_values[XYZ_AXIS_COUNT] = {0,0,0}; int16_t fake_acc_values[XYZ_AXIS_COUNT] = {0,0,0};
static void fakeAccInit(acc_t *acc) {UNUSED(acc);}
static void fakeAccInit(accDev_t *acc) {UNUSED(acc);}
static bool fakeAccRead(int16_t *accData) { static bool fakeAccRead(int16_t *accData) {
for(int i=0;i<XYZ_AXIS_COUNT;++i) { for(int i=0;i<XYZ_AXIS_COUNT;++i) {
accData[i] = fake_acc_values[i]; accData[i] = fake_acc_values[i];
@ -151,7 +147,7 @@ static bool fakeAccRead(int16_t *accData) {
return true; return true;
} }
bool fakeAccDetect(acc_t *acc) bool fakeAccDetect(accDev_t *acc)
{ {
acc->init = fakeAccInit; acc->init = fakeAccInit;
acc->read = fakeAccRead; acc->read = fakeAccRead;
@ -165,17 +161,17 @@ bool detectGyro(void)
{ {
gyroSensor_e gyroHardware = GYRO_DEFAULT; gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT; gyro.gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) { switch(gyroHardware) {
case GYRO_DEFAULT: case GYRO_DEFAULT:
; // fallthrough ; // fallthrough
case GYRO_MPU6050: case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050 #ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro)) { if (mpu6050GyroDetect(&gyro.dev)) {
gyroHardware = GYRO_MPU6050; gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN #ifdef GYRO_MPU6050_ALIGN
gyroAlign = GYRO_MPU6050_ALIGN; gyro.gyroAlign = GYRO_MPU6050_ALIGN;
#endif #endif
break; break;
} }
@ -183,10 +179,10 @@ bool detectGyro(void)
; // fallthrough ; // fallthrough
case GYRO_L3G4200D: case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D #ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro)) { if (l3g4200dDetect(&gyro.dev)) {
gyroHardware = GYRO_L3G4200D; gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN #ifdef GYRO_L3G4200D_ALIGN
gyroAlign = GYRO_L3G4200D_ALIGN; gyro.gyroAlign = GYRO_L3G4200D_ALIGN;
#endif #endif
break; break;
} }
@ -195,10 +191,10 @@ bool detectGyro(void)
case GYRO_MPU3050: case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050 #ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro)) { if (mpu3050Detect(&gyro.dev)) {
gyroHardware = GYRO_MPU3050; gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN #ifdef GYRO_MPU3050_ALIGN
gyroAlign = GYRO_MPU3050_ALIGN; gyro.gyroAlign = GYRO_MPU3050_ALIGN;
#endif #endif
break; break;
} }
@ -207,10 +203,10 @@ bool detectGyro(void)
case GYRO_L3GD20: case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20 #ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro)) { if (l3gd20Detect(&gyro.dev)) {
gyroHardware = GYRO_L3GD20; gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN #ifdef GYRO_L3GD20_ALIGN
gyroAlign = GYRO_L3GD20_ALIGN; gyro.gyroAlign = GYRO_L3GD20_ALIGN;
#endif #endif
break; break;
} }
@ -219,10 +215,10 @@ bool detectGyro(void)
case GYRO_MPU6000: case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000 #ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro)) { if (mpu6000SpiGyroDetect(&gyro.dev)) {
gyroHardware = GYRO_MPU6000; gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN #ifdef GYRO_MPU6000_ALIGN
gyroAlign = GYRO_MPU6000_ALIGN; gyro.gyroAlign = GYRO_MPU6000_ALIGN;
#endif #endif
break; break;
} }
@ -232,14 +228,14 @@ bool detectGyro(void)
case GYRO_MPU6500: case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500 #ifdef USE_GYRO_SPI_MPU6500
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) if (mpu6500GyroDetect(&gyro.dev) || mpu6500SpiGyroDetect(&gyro.dev))
#else #else
if (mpu6500GyroDetect(&gyro)) if (mpu6500GyroDetect(&gyro.dev))
#endif #endif
{ {
gyroHardware = GYRO_MPU6500; gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN #ifdef GYRO_MPU6500_ALIGN
gyroAlign = GYRO_MPU6500_ALIGN; gyro.gyroAlign = GYRO_MPU6500_ALIGN;
#endif #endif
break; break;
@ -250,11 +246,11 @@ bool detectGyro(void)
case GYRO_MPU9250: case GYRO_MPU9250:
#ifdef USE_GYRO_SPI_MPU9250 #ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiGyroDetect(&gyro)) if (mpu9250SpiGyroDetect(&gyro.dev))
{ {
gyroHardware = GYRO_MPU9250; gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN #ifdef GYRO_MPU9250_ALIGN
gyroAlign = GYRO_MPU9250_ALIGN; gyro.gyroAlign = GYRO_MPU9250_ALIGN;
#endif #endif
break; break;
@ -264,11 +260,11 @@ bool detectGyro(void)
case GYRO_ICM20689: case GYRO_ICM20689:
#ifdef USE_GYRO_SPI_ICM20689 #ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiGyroDetect(&gyro)) if (icm20689SpiGyroDetect(&gyro.dev))
{ {
gyroHardware = GYRO_ICM20689; gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN #ifdef GYRO_ICM20689_ALIGN
gyroAlign = GYRO_ICM20689_ALIGN; gyro.gyroAlign = GYRO_ICM20689_ALIGN;
#endif #endif
break; break;
@ -278,7 +274,7 @@ bool detectGyro(void)
case GYRO_FAKE: case GYRO_FAKE:
#ifdef USE_FAKE_GYRO #ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro)) { if (fakeGyroDetect(&gyro.dev)) {
gyroHardware = GYRO_FAKE; gyroHardware = GYRO_FAKE;
break; break;
} }
@ -307,7 +303,7 @@ static bool detectAcc(accelerationSensor_e accHardwareToUse)
#endif #endif
retry: retry:
accAlign = ALIGN_DEFAULT; acc.accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) { switch (accHardwareToUse) {
case ACC_DEFAULT: case ACC_DEFAULT:
@ -317,12 +313,12 @@ retry:
acc_params.useFifo = false; acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently acc_params.dataRate = 800; // unused currently
#ifdef NAZE #ifdef NAZE
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) { if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc.dev)) {
#else #else
if (adxl345Detect(&acc_params, &acc)) { if (adxl345Detect(&acc_params, &acc.dev)) {
#endif #endif
#ifdef ACC_ADXL345_ALIGN #ifdef ACC_ADXL345_ALIGN
accAlign = ACC_ADXL345_ALIGN; acc.accAlign = ACC_ADXL345_ALIGN;
#endif #endif
accHardware = ACC_ADXL345; accHardware = ACC_ADXL345;
break; break;
@ -331,9 +327,9 @@ retry:
; // fallthrough ; // fallthrough
case ACC_LSM303DLHC: case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC #ifdef USE_ACC_LSM303DLHC
if (lsm303dlhcAccDetect(&acc)) { if (lsm303dlhcAccDetect(&acc.dev)) {
#ifdef ACC_LSM303DLHC_ALIGN #ifdef ACC_LSM303DLHC_ALIGN
accAlign = ACC_LSM303DLHC_ALIGN; acc.accAlign = ACC_LSM303DLHC_ALIGN;
#endif #endif
accHardware = ACC_LSM303DLHC; accHardware = ACC_LSM303DLHC;
break; break;
@ -342,9 +338,9 @@ retry:
; // fallthrough ; // fallthrough
case ACC_MPU6050: // MPU6050 case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050 #ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(&acc)) { if (mpu6050AccDetect(&acc.dev)) {
#ifdef ACC_MPU6050_ALIGN #ifdef ACC_MPU6050_ALIGN
accAlign = ACC_MPU6050_ALIGN; acc.accAlign = ACC_MPU6050_ALIGN;
#endif #endif
accHardware = ACC_MPU6050; accHardware = ACC_MPU6050;
break; break;
@ -355,12 +351,12 @@ retry:
#ifdef USE_ACC_MMA8452 #ifdef USE_ACC_MMA8452
#ifdef NAZE #ifdef NAZE
// Not supported with this frequency // Not supported with this frequency
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) { if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc.dev)) {
#else #else
if (mma8452Detect(&acc)) { if (mma8452Detect(&acc.dev)) {
#endif #endif
#ifdef ACC_MMA8452_ALIGN #ifdef ACC_MMA8452_ALIGN
accAlign = ACC_MMA8452_ALIGN; acc.accAlign = ACC_MMA8452_ALIGN;
#endif #endif
accHardware = ACC_MMA8452; accHardware = ACC_MMA8452;
break; break;
@ -369,9 +365,9 @@ retry:
; // fallthrough ; // fallthrough
case ACC_BMA280: // BMA280 case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280 #ifdef USE_ACC_BMA280
if (bma280Detect(&acc)) { if (bma280Detect(&acc.dev)) {
#ifdef ACC_BMA280_ALIGN #ifdef ACC_BMA280_ALIGN
accAlign = ACC_BMA280_ALIGN; acc.accAlign = ACC_BMA280_ALIGN;
#endif #endif
accHardware = ACC_BMA280; accHardware = ACC_BMA280;
break; break;
@ -380,9 +376,9 @@ retry:
; // fallthrough ; // fallthrough
case ACC_MPU6000: case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000 #ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc)) { if (mpu6000SpiAccDetect(&acc.dev)) {
#ifdef ACC_MPU6000_ALIGN #ifdef ACC_MPU6000_ALIGN
accAlign = ACC_MPU6000_ALIGN; acc.accAlign = ACC_MPU6000_ALIGN;
#endif #endif
accHardware = ACC_MPU6000; accHardware = ACC_MPU6000;
break; break;
@ -392,13 +388,13 @@ retry:
case ACC_MPU6500: case ACC_MPU6500:
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
#ifdef USE_ACC_SPI_MPU6500 #ifdef USE_ACC_SPI_MPU6500
if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) if (mpu6500AccDetect(&acc.dev) || mpu6500SpiAccDetect(&acc.dev))
#else #else
if (mpu6500AccDetect(&acc)) if (mpu6500AccDetect(&acc.dev))
#endif #endif
{ {
#ifdef ACC_MPU6500_ALIGN #ifdef ACC_MPU6500_ALIGN
accAlign = ACC_MPU6500_ALIGN; acc.accAlign = ACC_MPU6500_ALIGN;
#endif #endif
accHardware = ACC_MPU6500; accHardware = ACC_MPU6500;
break; break;
@ -408,10 +404,10 @@ retry:
case ACC_ICM20689: case ACC_ICM20689:
#ifdef USE_ACC_SPI_ICM20689 #ifdef USE_ACC_SPI_ICM20689
if (icm20689SpiAccDetect(&acc)) if (icm20689SpiAccDetect(&acc.dev))
{ {
#ifdef ACC_ICM20689_ALIGN #ifdef ACC_ICM20689_ALIGN
accAlign = ACC_ICM20689_ALIGN; acc.accAlign = ACC_ICM20689_ALIGN;
#endif #endif
accHardware = ACC_ICM20689; accHardware = ACC_ICM20689;
break; break;
@ -420,7 +416,7 @@ retry:
; // fallthrough ; // fallthrough
case ACC_FAKE: case ACC_FAKE:
#ifdef USE_FAKE_ACC #ifdef USE_FAKE_ACC
if (fakeAccDetect(&acc)) { if (fakeAccDetect(&acc.dev)) {
accHardware = ACC_FAKE; accHardware = ACC_FAKE;
break; break;
} }
@ -480,7 +476,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough ; // fallthough
case BARO_BMP085: case BARO_BMP085:
#ifdef USE_BARO_BMP085 #ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro)) { if (bmp085Detect(bmp085Config, &baro.dev)) {
baroHardware = BARO_BMP085; baroHardware = BARO_BMP085;
break; break;
} }
@ -488,7 +484,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough ; // fallthough
case BARO_MS5611: case BARO_MS5611:
#ifdef USE_BARO_MS5611 #ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) { if (ms5611Detect(&baro.dev)) {
baroHardware = BARO_MS5611; baroHardware = BARO_MS5611;
break; break;
} }
@ -496,7 +492,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough ; // fallthough
case BARO_BMP280: case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280) #if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(&baro)) { if (bmp280Detect(&baro.dev)) {
baroHardware = BARO_BMP280; baroHardware = BARO_BMP280;
break; break;
} }
@ -551,7 +547,7 @@ static bool detectMag(magSensor_e magHardwareToUse)
retry: retry:
magAlign = ALIGN_DEFAULT; mag.magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) { switch(magHardwareToUse) {
case MAG_DEFAULT: case MAG_DEFAULT:
@ -559,9 +555,9 @@ retry:
case MAG_HMC5883: case MAG_HMC5883:
#ifdef USE_MAG_HMC5883 #ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag, hmc5883Config)) { if (hmc5883lDetect(&mag.dev, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN #ifdef MAG_HMC5883_ALIGN
magAlign = MAG_HMC5883_ALIGN; mag.magAlign = MAG_HMC5883_ALIGN;
#endif #endif
magHardware = MAG_HMC5883; magHardware = MAG_HMC5883;
break; break;
@ -571,9 +567,9 @@ retry:
case MAG_AK8975: case MAG_AK8975:
#ifdef USE_MAG_AK8975 #ifdef USE_MAG_AK8975
if (ak8975Detect(&mag)) { if (ak8975Detect(&mag.dev)) {
#ifdef MAG_AK8975_ALIGN #ifdef MAG_AK8975_ALIGN
magAlign = MAG_AK8975_ALIGN; mag.magAlign = MAG_AK8975_ALIGN;
#endif #endif
magHardware = MAG_AK8975; magHardware = MAG_AK8975;
break; break;
@ -583,9 +579,9 @@ retry:
case MAG_AK8963: case MAG_AK8963:
#ifdef USE_MAG_AK8963 #ifdef USE_MAG_AK8963
if (ak8963Detect(&mag)) { if (ak8963Detect(&mag.dev)) {
#ifdef MAG_AK8963_ALIGN #ifdef MAG_AK8963_ALIGN
magAlign = MAG_AK8963_ALIGN; mag.magAlign = MAG_AK8963_ALIGN;
#endif #endif
magHardware = MAG_AK8963; magHardware = MAG_AK8963;
break; break;
@ -630,13 +626,13 @@ static bool detectSonar(void)
static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig) static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
{ {
if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
gyroAlign = sensorAlignmentConfig->gyro_align; gyro.gyroAlign = sensorAlignmentConfig->gyro_align;
} }
if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) { if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
accAlign = sensorAlignmentConfig->acc_align; acc.accAlign = sensorAlignmentConfig->acc_align;
} }
if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) { if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) {
magAlign = sensorAlignmentConfig->mag_align; mag.magAlign = sensorAlignmentConfig->mag_align;
} }
} }
@ -664,25 +660,25 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
// Now time to init things // Now time to init things
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
gyro.lpf = gyroConfig->gyro_lpf; gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.init(&gyro); // driver initialisation gyro.dev.init(&gyro.dev); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation gyroInit(gyroConfig); // sensor initialisation
if (detectAcc(sensorSelectionConfig->acc_hardware)) { if (detectAcc(sensorSelectionConfig->acc_hardware)) {
acc.acc_1G = 256; // set default acc.dev.acc_1G = 256; // set default
acc.init(&acc); // driver initialisation acc.dev.init(&acc.dev); // driver initialisation
accInit(gyro.targetLooptime); // sensor initialisation accInit(gyro.targetLooptime); // sensor initialisation
} }
magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
#ifdef MAG #ifdef MAG
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (detectMag(sensorSelectionConfig->mag_hardware)) { if (detectMag(sensorSelectionConfig->mag_hardware)) {
// calculate magnetic declination // calculate magnetic declination
const int16_t deg = magDeclinationFromConfig / 100; const int16_t deg = magDeclinationFromConfig / 100;
const int16_t min = magDeclinationFromConfig % 100; const int16_t min = magDeclinationFromConfig % 100;
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
compassInit(); compassInit();
} }
#else #else

17
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -608,14 +608,17 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
case BST_RAW_IMU: case BST_RAW_IMU:
{ {
// Hack scale due to choice of units for sensor data in multiwii // Hack scale due to choice of units for sensor data in multiwii
uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1; uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1;
for (i = 0; i < 3; i++) for (i = 0; i < 3; i++) {
bstWrite16(accSmooth[i] / scale); bstWrite16(acc.accSmooth[i] / scale);
for (i = 0; i < 3; i++) }
bstWrite16(lrintf(gyroADCf[i])); for (i = 0; i < 3; i++) {
for (i = 0; i < 3; i++) bstWrite16(lrintf(gyro.gyroADCf[i] /gyro.dev.scale));
bstWrite16(magADC[i]); }
for (i = 0; i < 3; i++) {
bstWrite16(mag.magADC[i]);
}
} }
break; break;
#ifdef USE_SERVOS #ifdef USE_SERVOS

8
src/main/telemetry/frsky.c
View File

@ -166,16 +166,16 @@ static void sendAccel(void)
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i); sendDataHead(ID_ACC_X + i);
serialize16(((float)accSmooth[i] / acc.acc_1G) * 1000); serialize16(((float)acc.accSmooth[i] / acc.dev.acc_1G) * 1000);
} }
} }
static void sendBaro(void) static void sendBaro(void)
{ {
sendDataHead(ID_ALTITUDE_BP); sendDataHead(ID_ALTITUDE_BP);
serialize16(BaroAlt / 100); serialize16(baro.BaroAlt / 100);
sendDataHead(ID_ALTITUDE_AP); sendDataHead(ID_ALTITUDE_AP);
serialize16(ABS(BaroAlt % 100)); serialize16(ABS(baro.BaroAlt % 100));
} }
#ifdef GPS #ifdef GPS
@ -212,7 +212,7 @@ static void sendTemperature1(void)
{ {
sendDataHead(ID_TEMPRATURE1); sendDataHead(ID_TEMPRATURE1);
#ifdef BARO #ifdef BARO
serialize16((baroTemperature + 50)/ 100); //Airmamaf serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
#else #else
serialize16(telemTemperature1 / 10); serialize16(telemTemperature1 / 10);
#endif #endif

8
src/main/telemetry/smartport.c
View File

@ -654,7 +654,7 @@ void handleSmartPortTelemetry(void)
//case FSSP_DATAID_RPM : //case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE : case FSSP_DATAID_ALTITUDE :
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter smartPortSendPackage(id, baro.BaroAlt); // unknown given unit, requested 100 = 1 meter
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
break; break;
@ -700,15 +700,15 @@ void handleSmartPortTelemetry(void)
smartPortHasRequest = 0; smartPortHasRequest = 0;
break; break;
case FSSP_DATAID_ACCX : case FSSP_DATAID_ACCX :
smartPortSendPackage(id, 100 * accSmooth[X] / acc.acc_1G); // Multiply by 100 to show as x.xx g on Taranis smartPortSendPackage(id, 100 * acc.accSmooth[X] / acc.dev.acc_1G); // Multiply by 100 to show as x.xx g on Taranis
smartPortHasRequest = 0; smartPortHasRequest = 0;
break; break;
case FSSP_DATAID_ACCY : case FSSP_DATAID_ACCY :
smartPortSendPackage(id, 100 * accSmooth[Y] / acc.acc_1G); smartPortSendPackage(id, 100 * acc.accSmooth[Y] / acc.dev.acc_1G);
smartPortHasRequest = 0; smartPortHasRequest = 0;
break; break;
case FSSP_DATAID_ACCZ : case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, 100 * accSmooth[Z] / acc.acc_1G); smartPortSendPackage(id, 100 * acc.accSmooth[Z] / acc.dev.acc_1G);
smartPortHasRequest = 0; smartPortHasRequest = 0;
break; break;
case FSSP_DATAID_T1 : case FSSP_DATAID_T1 :