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

Merge pull request #1715 from martinbudden/bf_sensor_structs 

Moved sensor global data into sensor_s structs
This commit is contained in:
Martin Budden 2016-12-01 19:06:04 +01:00 committed by GitHub
parent fd5710051e b8b9c95f57
commit 4ec9efada8
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++) {
blackboxCurrent->gyroADC[i] = lrintf(gyroADCf[i]);
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[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++) {
@ -1011,12 +1011,12 @@ static void loadMainState(uint32_t currentTime)
#ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->magADC[i] = magADC[i];
blackboxCurrent->magADC[i] = mag.magADC[i];
}
#endif
#ifdef BARO
blackboxCurrent->BaroAlt = BaroAlt;
blackboxCurrent->BaroAlt = baro.BaroAlt;
#endif
#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("minthrottle:%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x", castFloatBytesToInt(gyro.dev.scale));
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {

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

@ -18,7 +18,6 @@
#pragma once
#include "common/axis.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#ifndef MPU_I2C_INSTANCE
@ -34,21 +33,20 @@
#define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7
typedef struct gyro_s {
typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function
sensorGyroReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
float scale; // scalefactor
uint32_t targetLooptime;
uint16_t lpf;
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
sensorReadFuncPtr read; // read 3 axis data function
uint16_t acc_1G;
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_FIFO_STREAM 0x80
static void adxl345Init(acc_t *acc);
static void adxl345Init(accDev_t *acc);
static bool adxl345Read(int16_t *accelData);
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;
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;
}
static void adxl345Init(acc_t *acc)
static void adxl345Init(accDev_t *acc)
{
if (useFifo) {
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;
} 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_RANGE 0x0F
static void bma280Init(acc_t *acc);
static void bma280Init(accDev_t *acc);
static bool bma280Read(int16_t *accelData);
bool bma280Detect(acc_t *acc)
bool bma280Detect(accDev_t *acc)
{
uint8_t sig = 0;
bool ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
@ -48,7 +48,7 @@ bool bma280Detect(acc_t *acc)
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_BW, 0x0E); // 500Hz BW

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

@ -17,4 +17,4 @@
#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_93HZ 0xC0
static void l3g4200dInit(gyro_t *gyro);
static bool l3g4200dRead(gyro_t *gyro);
static void l3g4200dInit(gyroDev_t *gyro);
static bool l3g4200dRead(gyroDev_t *gyro);
bool l3g4200dDetect(gyro_t *gyro)
bool l3g4200dDetect(gyroDev_t *gyro)
{
uint8_t deviceid;
@ -76,7 +76,7 @@ bool l3g4200dDetect(gyro_t *gyro)
return true;
}
static void l3g4200dInit(gyro_t *gyro)
static void l3g4200dInit(gyroDev_t *gyro)
{
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.
static bool l3g4200dRead(gyro_t *gyro)
static bool l3g4200dRead(gyroDev_t *gyro)
{
uint8_t buf[6];

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

@ -17,4 +17,4 @@
#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);
}
void l3gd20GyroInit(gyro_t *gyro)
void l3gd20GyroInit(gyroDev_t *gyro)
{
UNUSED(gyro); // FIXME use it!
@ -120,7 +120,7 @@ void l3gd20GyroInit(gyro_t *gyro)
delay(100);
}
static bool l3gd20GyroRead(gyro_t *gyro)
static bool l3gd20GyroRead(gyroDev_t *gyro)
{
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
#define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyro_t *gyro)
bool l3gd20Detect(gyroDev_t *gyro)
{
gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead;

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

@ -17,4 +17,4 @@
#pragma once
bool l3gd20Detect(gyro_t *gyro);
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];
void lsm303dlhcAccInit(acc_t *acc)
void lsm303dlhcAccInit(accDev_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
@ -157,7 +157,7 @@ static bool lsm303dlhcAccRead(int16_t *gyroADC)
return true;
}
bool lsm303dlhcAccDetect(acc_t *acc)
bool lsm303dlhcAccDetect(accDev_t *acc)
{
bool ack;
uint8_t status;

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

@ -195,10 +195,10 @@ typedef struct {
/** @defgroup Acc_Full_Scale_Selection
* @{
*/
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< ±2 g */
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< ±4 g */
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< ±8 g */
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< ±16 g */
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< +/-2 g */
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< +/-4 g */
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< +/-8 g */
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< +/-16 g */
/**
* @}
*/
@ -388,13 +388,13 @@ typedef struct {
/** @defgroup Mag_Full_Scale
* @{
*/
#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_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_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_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = ±8.1 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_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_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_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 */
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 void mma8452Init(acc_t *acc);
static void mma8452Init(accDev_t *acc);
static bool mma8452Read(int16_t *accelData);
bool mma8452Detect(acc_t *acc)
bool mma8452Detect(accDev_t *acc)
{
uint8_t sig = 0;
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
}
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

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

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

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

@ -17,7 +17,6 @@
#pragma once
#include "io_types.h"
#include "exti.h"
// MPU6050
@ -185,9 +184,9 @@ typedef struct mpuDetectionResult_s {
extern mpuDetectionResult_t mpuDetectionResult;
void configureMPUDataReadyInterruptHandling(void);
struct gyro_s;
void mpuGyroInit(struct gyro_s *gyro);
struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro);
bool mpuAccRead(int16_t *accData);
bool mpuGyroRead(struct gyro_s *gyro);
bool mpuGyroRead(struct gyroDev_s *gyro);
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_CLK_SEL_PLL_GX 0x01
static void mpu3050Init(gyro_t *gyro);
static void mpu3050Init(gyroDev_t *gyro);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro)
bool mpu3050Detect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_3050) {
return false;
@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
return true;
}
static void mpu3050Init(gyro_t *gyro)
static void mpu3050Init(gyroDev_t *gyro)
{
bool ack;

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

@ -26,4 +26,4 @@
#define MPU3050_USER_CTRL 0x3D
#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
static void mpu6050AccInit(acc_t *acc);
static void mpu6050GyroInit(gyro_t *gyro);
static void mpu6050AccInit(accDev_t *acc);
static void mpu6050GyroInit(gyroDev_t *gyro);
bool mpu6050AccDetect(acc_t *acc)
bool mpu6050AccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;
@ -66,7 +66,7 @@ bool mpu6050AccDetect(acc_t *acc)
return true;
}
bool mpu6050GyroDetect(gyro_t *gyro)
bool mpu6050GyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;
@ -81,7 +81,7 @@ bool mpu6050GyroDetect(gyro_t *gyro)
return true;
}
static void mpu6050AccInit(acc_t *acc)
static void mpu6050AccInit(accDev_t *acc)
{
switch (mpuDetectionResult.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);

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

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

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

@ -34,7 +34,7 @@
#include "accgyro_mpu.h"
#include "accgyro_mpu6500.h"
bool mpu6500AccDetect(acc_t *acc)
bool mpu6500AccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
@ -46,7 +46,7 @@ bool mpu6500AccDetect(acc_t *acc)
return true;
}
bool mpu6500GyroDetect(gyro_t *gyro)
bool mpu6500GyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
@ -62,12 +62,12 @@ bool mpu6500GyroDetect(gyro_t *gyro)
return true;
}
void mpu6500AccInit(acc_t *acc)
void mpu6500AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 4;
}
void mpu6500GyroInit(gyro_t *gyro)
void mpu6500GyroInit(gyroDev_t *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_RAW_RDY_EN (0x01)
bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro);
bool mpu6500AccDetect(accDev_t *acc);
bool mpu6500GyroDetect(gyroDev_t *gyro);
void mpu6500AccInit(acc_t *acc);
void mpu6500GyroInit(gyro_t *gyro);
void mpu6500AccInit(accDev_t *acc);
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) {
return false;
@ -119,7 +119,7 @@ bool icm20689SpiAccDetect(acc_t *acc)
return true;
}
bool icm20689SpiGyroDetect(gyro_t *gyro)
bool icm20689SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != ICM_20689_SPI) {
return false;
@ -135,12 +135,12 @@ bool icm20689SpiGyroDetect(gyro_t *gyro)
return true;
}
void icm20689AccInit(acc_t *acc)
void icm20689AccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 4;
}
void icm20689GyroInit(gyro_t *gyro)
void icm20689GyroInit(gyroDev_t *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_BIT_RESET (0x80)
bool icm20689AccDetect(acc_t *acc);
bool icm20689GyroDetect(gyro_t *gyro);
bool icm20689AccDetect(accDev_t *acc);
bool icm20689GyroDetect(gyroDev_t *gyro);
void icm20689AccInit(acc_t *acc);
void icm20689GyroInit(gyro_t *gyro);
void icm20689AccInit(accDev_t *acc);
void icm20689GyroInit(gyroDev_t *gyro);
bool icm20689SpiDetect(void);
bool icm20689SpiAccDetect(acc_t *acc);
bool icm20689SpiGyroDetect(gyro_t *gyro);
bool icm20689SpiAccDetect(accDev_t *acc);
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
bool icm20689WriteRegister(uint8_t reg, 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;
}
void mpu6000SpiGyroInit(gyro_t *gyro)
void mpu6000SpiGyroInit(gyroDev_t *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;
}
@ -254,7 +254,7 @@ static void mpu6000AccAndGyroInit(void)
mpuSpi6000InitDone = true;
}
bool mpu6000SpiAccDetect(acc_t *acc)
bool mpu6000SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
@ -266,7 +266,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
return true;
}
bool mpu6000SpiGyroDetect(gyro_t *gyro)
bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;

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

@ -17,8 +17,8 @@
bool mpu6000SpiDetect(void);
bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro);
bool mpu6000SpiAccDetect(accDev_t *acc);
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
bool mpu6000WriteRegister(uint8_t reg, 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;
}
void mpu6500SpiAccInit(acc_t *acc)
void mpu6500SpiAccInit(accDev_t *acc)
{
mpu6500AccInit(acc);
}
void mpu6500SpiGyroInit(gyro_t *gyro)
void mpu6500SpiGyroInit(gyroDev_t *gyro)
{
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
delayMicroseconds(1);
@ -114,7 +114,7 @@ void mpu6500SpiGyroInit(gyro_t *gyro)
delayMicroseconds(1);
}
bool mpu6500SpiAccDetect(acc_t *acc)
bool mpu6500SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
@ -126,7 +126,7 @@ bool mpu6500SpiAccDetect(acc_t *acc)
return true;
}
bool mpu6500SpiGyroDetect(gyro_t *gyro)
bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;

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

@ -19,11 +19,11 @@
bool mpu6500SpiDetect(void);
void mpu6500SpiAccInit(acc_t *acc);
void mpu6500SpiGyroInit(gyro_t *gyro);
void mpu6500SpiAccInit(accDev_t *acc);
void mpu6500SpiGyroInit(gyroDev_t *gyro);
bool mpu6500SpiAccDetect(acc_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro);
bool mpu6500SpiAccDetect(accDev_t *acc);
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
bool mpu6500WriteRegister(uint8_t reg, 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;
}
void mpu9250SpiGyroInit(gyro_t *gyro)
void mpu9250SpiGyroInit(gyroDev_t *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;
}
@ -209,7 +209,7 @@ bool mpu9250SpiDetect(void)
return true;
}
bool mpu9250SpiAccDetect(acc_t *acc)
bool mpu9250SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;
@ -221,7 +221,7 @@ bool mpu9250SpiAccDetect(acc_t *acc)
return true;
}
bool mpu9250SpiGyroDetect(gyro_t *gyro)
bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;

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

@ -27,8 +27,8 @@ void mpu9250ResetGyro(void);
bool mpu9250SpiDetect(void);
bool mpu9250SpiAccDetect(acc_t *acc);
bool mpu9250SpiGyroDetect(gyro_t *gyro);
bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250WriteRegister(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 (*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 up_delay;
baroOpFuncPtr start_ut;
@ -28,7 +28,7 @@ typedef struct baro_s {
baroOpFuncPtr start_up;
baroOpFuncPtr get_up;
baroCalculateFuncPtr calculate;
} baro_t;
} baroDev_t;
#ifndef BARO_I2C_INSTANCE
#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;
}
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
{
uint8_t data;
bool ack;

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

@ -22,7 +22,7 @@ typedef struct bmp085Config_s {
ioTag_t eocIO;
} 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);
#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
STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
bool bmp280Detect(baro_t *baro)
bool bmp280Detect(baroDev_t *baro)
{
if (bmp280InitDone)
return true;

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

@ -56,5 +56,5 @@
#define T_SETUP_PRESSURE_MAX (10)
// 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 uint8_t ms5611_osr = CMD_ADC_4096;
bool ms5611Detect(baro_t *baro)
bool ms5611Detect(baroDev_t *baro)
{
uint8_t sig;
int i;

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

@ -17,4 +17,4 @@
#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"
typedef struct mag_s {
typedef struct magDev_s {
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
} mag_t;
} magDev_t;
#ifndef MAG_I2C_INSTANCE
#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
bool ak8963Detect(mag_t *mag)
bool ak8963Detect(magDev_t *mag)
{
uint8_t sig = 0;

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

@ -17,6 +17,6 @@
#pragma once
bool ak8963Detect(mag_t *mag);
bool ak8963Detect(magDev_t *mag);
void ak8963Init(void);
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_ASCT 0x0c // self test
bool ak8975Detect(mag_t *mag)
bool ak8975Detect(magDev_t *mag)
{
uint8_t sig = 0;
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
bool ak8975Detect(mag_t *mag);
bool ak8975Detect(magDev_t *mag);
void ak8975Init(void);
bool ak8975Read(int16_t *magData);

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

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

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

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

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

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

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

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

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

@ -17,12 +17,12 @@
#pragma once
struct acc_s;
struct accDev_s;
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
struct gyro_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyro_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyro_s *gyro);
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyro_s *gyro);
typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc); // sensor init prototype
struct gyroDev_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
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:
{
// 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++) {
sbufWriteU16(dst, accSmooth[i] / scale);
sbufWriteU16(dst, acc.accSmooth[i] / scale);
}
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++) {
sbufWriteU16(dst, magADC[i]);
sbufWriteU16(dst, mag.magADC[i]);
}
}
break;

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

@ -219,7 +219,7 @@ void fcTasksInit(void)
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
rescheduleTask(TASK_ACCEL, accSamplingInterval);
rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
}
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();
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature) {
gyro.temperature(&telemTemperature1);
if (gyro.dev.temperature) {
gyro.dev.temperature(&telemTemperature1);
}
#ifdef MAG

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

@ -237,9 +237,9 @@ void calculateEstimatedAltitude(uint32_t currentTime)
accAlt = 0;
}
BaroAlt = baroCalculateAltitude();
baro.BaroAlt = baroCalculateAltitude();
#else
BaroAlt = 0;
baro.BaroAlt = 0;
#endif
#ifdef SONAR
@ -248,14 +248,14 @@ void calculateEstimatedAltitude(uint32_t currentTime)
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// just use the SONAR
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
baroAlt_offset = baro.BaroAlt - sonarAlt;
baro.BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
baro.BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
baro.BaroAlt = sonarAlt * sonarTransition + baro.BaroAlt * (1.0f - sonarTransition);
}
}
#endif
@ -272,7 +272,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
// Integrator - Altitude in cm
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;
#ifdef DEBUG_ALT_HOLD
@ -292,7 +292,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
#ifdef SONAR
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// the sonar has the best range
EstAlt = BaroAlt;
EstAlt = baro.BaroAlt;
} else {
EstAlt = accAlt;
}
@ -300,8 +300,8 @@ void calculateEstimatedAltitude(uint32_t currentTime)
EstAlt = accAlt;
#endif
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = (baro.BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baro.BaroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
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)
{
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();
}
@ -172,9 +172,9 @@ void imuCalculateAcceleration(uint32_t deltaT)
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
accel_ned.V.X = acc.accSmooth[X];
accel_ned.V.Y = acc.accSmooth[Y];
accel_ned.V.Z = acc.accSmooth[Z];
imuTransformVectorBodyToEarth(&accel_ned);
@ -185,7 +185,7 @@ void imuCalculateAcceleration(uint32_t deltaT)
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} 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
@ -355,10 +355,10 @@ static bool imuIsAccelerometerHealthy(void)
int32_t accMagnitude = 0;
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
return (81 < accMagnitude) && (accMagnitude < 121);
@ -366,7 +366,7 @@ static bool imuIsAccelerometerHealthy(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)
@ -396,9 +396,9 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroADCf[X]), DEGREES_TO_RADIANS(gyroADCf[Y]), DEGREES_TO_RADIANS(gyroADCf[Z]),
useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
useMag, magADC[X], magADC[Y], magADC[Z],
DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
useYaw, rawYawError);
imuUpdateEulerAngles();
@ -419,9 +419,9 @@ void imuUpdateAttitude(uint32_t currentTime)
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude(currentTime);
} else {
accSmooth[X] = 0;
accSmooth[Y] = 0;
accSmooth[Z] = 0;
acc.accSmooth[X] = 0;
acc.accSmooth[Y] = 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. ----------
// ---------- 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");
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();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
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();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
@ -504,7 +504,7 @@ void showSensorsPage(void)
#ifdef 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();
i2c_OLED_set_line(rowIndex++);
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:
{
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());
break;
}
@ -451,7 +451,7 @@ void osdUpdateAlarms(void)
// This is overdone?
// uint16_t *itemPos = osdProfile()->item_pos;
int32_t alt = osdGetAltitude(BaroAlt) / 100;
int32_t alt = osdGetAltitude(baro.BaroAlt) / 100;
statRssi = rssi * 100 / 1024;
if (statRssi < pOsdProfile->rssi_alarm)
@ -525,8 +525,8 @@ static void osdUpdateStats(void)
if (stats.min_rssi > statRssi)
stats.min_rssi = statRssi;
if (stats.max_altitude < BaroAlt)
stats.max_altitude = BaroAlt;
if (stats.max_altitude < baro.BaroAlt)
stats.max_altitude = baro.BaroAlt;
}
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);
if (mask == SENSOR_ACC && acc.revisionCode) {
cliPrintf(".%c", acc.revisionCode);
if (mask == SENSOR_ACC && acc.dev.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)) {
jetiExSensors[EX_VOLTAGE].value = vbat;
jetiExSensors[EX_CURRENT].value = amperage;
jetiExSensors[EX_ALTITUDE].value = BaroAlt;
jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;
jetiExSensors[EX_CAPACITY].value = mAhDrawn;
jetiExSensors[EX_FRAMES_LOST].value = framesLost;
jetiExSensors[EX_TIME_DIFF].value = timeDiff;

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

@ -39,11 +39,7 @@
#include "config/feature.h"
int32_t accSmooth[XYZ_AXIS_COUNT];
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.
@ -65,19 +61,19 @@ void accInit(uint32_t gyroSamplingInverval)
case 375:
case 250:
case 125:
accSamplingInterval = 1000;
acc.accSamplingInterval = 1000;
break;
case 1000:
default:
#ifdef STM32F10X
accSamplingInterval = 1000;
acc.accSamplingInterval = 1000;
#else
accSamplingInterval = 1000;
acc.accSamplingInterval = 1000;
#endif
}
if (accLpfCutHz) {
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;
// 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
accSmooth[axis] = 0;
acc.accSmooth[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
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[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);
@ -161,9 +157,9 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accSmooth[axis];
b[axis] += acc.accSmooth[axis];
// Clear global variables for next reading
accSmooth[axis] = 0;
acc.accSmooth[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
// all values are measured
@ -185,7 +181,7 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP
AccInflightCalibrationSavetoEEProm = false;
accelerationTrims->raw[X] = b[X] / 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);
@ -195,31 +191,31 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP
static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
{
accSmooth[X] -= accelerationTrims->raw[X];
accSmooth[Y] -= accelerationTrims->raw[Y];
accSmooth[Z] -= accelerationTrims->raw[Z];
acc.accSmooth[X] -= accelerationTrims->raw[X];
acc.accSmooth[Y] -= accelerationTrims->raw[Y];
acc.accSmooth[Z] -= accelerationTrims->raw[Z];
}
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
int16_t accADCRaw[XYZ_AXIS_COUNT];
if (!acc.read(accADCRaw)) {
if (!acc.dev.read(accADCRaw)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
accSmooth[axis] = accADCRaw[axis];
acc.accSmooth[axis] = accADCRaw[axis];
}
if (accLpfCutHz) {
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()) {
performAcclerationCalibration(rollAndPitchTrims);
@ -240,9 +236,9 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
void setAccelerationFilter(uint16_t initialAccLpfCutHz)
{
accLpfCutHz = initialAccLpfCutHz;
if (accSamplingInterval) {
if (acc.accSamplingInterval) {
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
} accelerationSensor_e;
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint32_t accSamplingInterval;
typedef struct acc_s {
accDev_t dev;
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 {
int16_t roll;

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

@ -21,9 +21,6 @@
#include "platform.h"
int32_t BaroAlt = 0;
#ifdef BARO
#include "common/maths.h"
#include "drivers/barometer.h"
@ -32,9 +29,12 @@ int32_t BaroAlt = 0;
#include "sensors/barometer.h"
baro_t baro; // barometer access functions
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
#ifdef BARO
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 baroGroundPressure = 0;
@ -126,19 +126,19 @@ uint32_t baroUpdate(void)
switch (state) {
default:
case BAROMETER_NEEDS_SAMPLES:
baro.get_ut();
baro.start_up();
baro.dev.get_ut();
baro.dev.start_up();
state = BAROMETER_NEEDS_CALCULATION;
return baro.up_delay;
return baro.dev.up_delay;
break;
case BAROMETER_NEEDS_CALCULATION:
baro.get_up();
baro.start_ut();
baro.calculate(&baroPressure, &baroTemperature);
baro.dev.get_up();
baro.dev.start_ut();
baro.dev.calculate(&baroPressure, &baroTemperature);
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_SAMPLES;
return baro.ut_delay;
return baro.dev.ut_delay;
break;
}
}
@ -151,9 +151,9 @@ int32_t baroCalculateAltitude(void)
// 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 -= 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)

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

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

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

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

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

@ -19,6 +19,8 @@
#include "drivers/compass.h"
#include "sensors/sensors.h"
// Type of magnetometer used/detected
typedef enum {
@ -30,6 +32,15 @@ typedef enum {
MAG_MAX = MAG_AK8963
} 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 {
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.
@ -39,7 +50,3 @@ void compassInit(void);
union flightDynamicsTrims_u;
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"
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
static int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
@ -175,15 +174,15 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(&gyro)) {
if (!gyro.dev.read(&gyro.dev)) {
return;
}
gyro.dataReady = false;
gyroADC[X] = gyro.gyroADCRaw[X];
gyroADC[Y] = gyro.gyroADCRaw[Y];
gyroADC[Z] = gyro.gyroADCRaw[Z];
gyro.dev.dataReady = false;
gyroADC[X] = gyro.dev.gyroADCRaw[X];
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
alignSensors(gyroADC, gyroAlign);
alignSensors(gyroADC, gyro.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete();
if (!calibrationComplete) {
@ -193,20 +192,24 @@ void gyroUpdate(void)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
// 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) {
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
} 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 {
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"
#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 };
@ -102,12 +96,12 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
#ifdef USE_FAKE_GYRO
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);
}
static bool fakeGyroRead(gyro_t *gyro)
static bool fakeGyroRead(gyroDev_t *gyro)
{
for (int i = 0; i < XYZ_AXIS_COUNT; ++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);
return true;
}
bool fakeGyroDetect(gyro_t *gyro)
bool fakeGyroDetect(gyroDev_t *gyro)
{
gyro->init = fakeGyroInit;
gyro->intStatus = fakeGyroInitStatus;
@ -142,7 +136,9 @@ bool fakeGyroDetect(gyro_t *gyro)
#ifdef USE_FAKE_ACC
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) {
for(int i=0;i<XYZ_AXIS_COUNT;++i) {
accData[i] = fake_acc_values[i];
@ -151,7 +147,7 @@ static bool fakeAccRead(int16_t *accData) {
return true;
}
bool fakeAccDetect(acc_t *acc)
bool fakeAccDetect(accDev_t *acc)
{
acc->init = fakeAccInit;
acc->read = fakeAccRead;
@ -165,17 +161,17 @@ bool detectGyro(void)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
gyro.gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro)) {
if (mpu6050GyroDetect(&gyro.dev)) {
gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
gyroAlign = GYRO_MPU6050_ALIGN;
gyro.gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
@ -183,10 +179,10 @@ bool detectGyro(void)
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro)) {
if (l3g4200dDetect(&gyro.dev)) {
gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN
gyroAlign = GYRO_L3G4200D_ALIGN;
gyro.gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
@ -195,10 +191,10 @@ bool detectGyro(void)
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro)) {
if (mpu3050Detect(&gyro.dev)) {
gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN
gyroAlign = GYRO_MPU3050_ALIGN;
gyro.gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
@ -207,10 +203,10 @@ bool detectGyro(void)
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro)) {
if (l3gd20Detect(&gyro.dev)) {
gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN
gyroAlign = GYRO_L3GD20_ALIGN;
gyro.gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
@ -219,10 +215,10 @@ bool detectGyro(void)
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro)) {
if (mpu6000SpiGyroDetect(&gyro.dev)) {
gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN
gyroAlign = GYRO_MPU6000_ALIGN;
gyro.gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
@ -232,14 +228,14 @@ bool detectGyro(void)
case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
if (mpu6500GyroDetect(&gyro.dev) || mpu6500SpiGyroDetect(&gyro.dev))
#else
if (mpu6500GyroDetect(&gyro))
if (mpu6500GyroDetect(&gyro.dev))
#endif
{
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
gyroAlign = GYRO_MPU6500_ALIGN;
gyro.gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
@ -250,11 +246,11 @@ bool detectGyro(void)
case GYRO_MPU9250:
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiGyroDetect(&gyro))
if (mpu9250SpiGyroDetect(&gyro.dev))
{
gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN
gyroAlign = GYRO_MPU9250_ALIGN;
gyro.gyroAlign = GYRO_MPU9250_ALIGN;
#endif
break;
@ -264,11 +260,11 @@ bool detectGyro(void)
case GYRO_ICM20689:
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiGyroDetect(&gyro))
if (icm20689SpiGyroDetect(&gyro.dev))
{
gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN
gyroAlign = GYRO_ICM20689_ALIGN;
gyro.gyroAlign = GYRO_ICM20689_ALIGN;
#endif
break;
@ -278,7 +274,7 @@ bool detectGyro(void)
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro)) {
if (fakeGyroDetect(&gyro.dev)) {
gyroHardware = GYRO_FAKE;
break;
}
@ -307,7 +303,7 @@ static bool detectAcc(accelerationSensor_e accHardwareToUse)
#endif
retry:
accAlign = ALIGN_DEFAULT;
acc.accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) {
case ACC_DEFAULT:
@ -317,12 +313,12 @@ retry:
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
#ifdef NAZE
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc.dev)) {
#else
if (adxl345Detect(&acc_params, &acc)) {
if (adxl345Detect(&acc_params, &acc.dev)) {
#endif
#ifdef ACC_ADXL345_ALIGN
accAlign = ACC_ADXL345_ALIGN;
acc.accAlign = ACC_ADXL345_ALIGN;
#endif
accHardware = ACC_ADXL345;
break;
@ -331,9 +327,9 @@ retry:
; // fallthrough
case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
if (lsm303dlhcAccDetect(&acc)) {
if (lsm303dlhcAccDetect(&acc.dev)) {
#ifdef ACC_LSM303DLHC_ALIGN
accAlign = ACC_LSM303DLHC_ALIGN;
acc.accAlign = ACC_LSM303DLHC_ALIGN;
#endif
accHardware = ACC_LSM303DLHC;
break;
@ -342,9 +338,9 @@ retry:
; // fallthrough
case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(&acc)) {
if (mpu6050AccDetect(&acc.dev)) {
#ifdef ACC_MPU6050_ALIGN
accAlign = ACC_MPU6050_ALIGN;
acc.accAlign = ACC_MPU6050_ALIGN;
#endif
accHardware = ACC_MPU6050;
break;
@ -355,12 +351,12 @@ retry:
#ifdef USE_ACC_MMA8452
#ifdef NAZE
// Not supported with this frequency
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc.dev)) {
#else
if (mma8452Detect(&acc)) {
if (mma8452Detect(&acc.dev)) {
#endif
#ifdef ACC_MMA8452_ALIGN
accAlign = ACC_MMA8452_ALIGN;
acc.accAlign = ACC_MMA8452_ALIGN;
#endif
accHardware = ACC_MMA8452;
break;
@ -369,9 +365,9 @@ retry:
; // fallthrough
case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
if (bma280Detect(&acc)) {
if (bma280Detect(&acc.dev)) {
#ifdef ACC_BMA280_ALIGN
accAlign = ACC_BMA280_ALIGN;
acc.accAlign = ACC_BMA280_ALIGN;
#endif
accHardware = ACC_BMA280;
break;
@ -380,9 +376,9 @@ retry:
; // fallthrough
case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc)) {
if (mpu6000SpiAccDetect(&acc.dev)) {
#ifdef ACC_MPU6000_ALIGN
accAlign = ACC_MPU6000_ALIGN;
acc.accAlign = ACC_MPU6000_ALIGN;
#endif
accHardware = ACC_MPU6000;
break;
@ -392,13 +388,13 @@ retry:
case ACC_MPU6500:
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
#ifdef USE_ACC_SPI_MPU6500
if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
if (mpu6500AccDetect(&acc.dev) || mpu6500SpiAccDetect(&acc.dev))
#else
if (mpu6500AccDetect(&acc))
if (mpu6500AccDetect(&acc.dev))
#endif
{
#ifdef ACC_MPU6500_ALIGN
accAlign = ACC_MPU6500_ALIGN;
acc.accAlign = ACC_MPU6500_ALIGN;
#endif
accHardware = ACC_MPU6500;
break;
@ -408,10 +404,10 @@ retry:
case ACC_ICM20689:
#ifdef USE_ACC_SPI_ICM20689
if (icm20689SpiAccDetect(&acc))
if (icm20689SpiAccDetect(&acc.dev))
{
#ifdef ACC_ICM20689_ALIGN
accAlign = ACC_ICM20689_ALIGN;
acc.accAlign = ACC_ICM20689_ALIGN;
#endif
accHardware = ACC_ICM20689;
break;
@ -420,7 +416,7 @@ retry:
; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
if (fakeAccDetect(&acc)) {
if (fakeAccDetect(&acc.dev)) {
accHardware = ACC_FAKE;
break;
}
@ -480,7 +476,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro)) {
if (bmp085Detect(bmp085Config, &baro.dev)) {
baroHardware = BARO_BMP085;
break;
}
@ -488,7 +484,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) {
if (ms5611Detect(&baro.dev)) {
baroHardware = BARO_MS5611;
break;
}
@ -496,7 +492,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(&baro)) {
if (bmp280Detect(&baro.dev)) {
baroHardware = BARO_BMP280;
break;
}
@ -551,7 +547,7 @@ static bool detectMag(magSensor_e magHardwareToUse)
retry:
magAlign = ALIGN_DEFAULT;
mag.magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) {
case MAG_DEFAULT:
@ -559,9 +555,9 @@ retry:
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag, hmc5883Config)) {
if (hmc5883lDetect(&mag.dev, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
magAlign = MAG_HMC5883_ALIGN;
mag.magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
break;
@ -571,9 +567,9 @@ retry:
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975Detect(&mag)) {
if (ak8975Detect(&mag.dev)) {
#ifdef MAG_AK8975_ALIGN
magAlign = MAG_AK8975_ALIGN;
mag.magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
break;
@ -583,9 +579,9 @@ retry:
case MAG_AK8963:
#ifdef USE_MAG_AK8963
if (ak8963Detect(&mag)) {
if (ak8963Detect(&mag.dev)) {
#ifdef MAG_AK8963_ALIGN
magAlign = MAG_AK8963_ALIGN;
mag.magAlign = MAG_AK8963_ALIGN;
#endif
magHardware = MAG_AK8963;
break;
@ -630,13 +626,13 @@ static bool detectSonar(void)
static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
{
if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
gyroAlign = sensorAlignmentConfig->gyro_align;
gyro.gyroAlign = sensorAlignmentConfig->gyro_align;
}
if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
accAlign = sensorAlignmentConfig->acc_align;
acc.accAlign = sensorAlignmentConfig->acc_align;
}
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
// 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.lpf = gyroConfig->gyro_lpf;
gyro.init(&gyro); // driver initialisation
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.dev.init(&gyro.dev); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation
if (detectAcc(sensorSelectionConfig->acc_hardware)) {
acc.acc_1G = 256; // set default
acc.init(&acc); // driver initialisation
acc.dev.acc_1G = 256; // set default
acc.dev.init(&acc.dev); // driver 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
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (detectMag(sensorSelectionConfig->mag_hardware)) {
// calculate magnetic declination
const int16_t deg = 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();
}
#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:
{
// 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++)
bstWrite16(accSmooth[i] / scale);
for (i = 0; i < 3; i++)
bstWrite16(lrintf(gyroADCf[i]));
for (i = 0; i < 3; i++)
bstWrite16(magADC[i]);
for (i = 0; i < 3; i++) {
bstWrite16(acc.accSmooth[i] / scale);
}
for (i = 0; i < 3; i++) {
bstWrite16(lrintf(gyro.gyroADCf[i] /gyro.dev.scale));
}
for (i = 0; i < 3; i++) {
bstWrite16(mag.magADC[i]);
}
}
break;
#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++) {
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)
{
sendDataHead(ID_ALTITUDE_BP);
serialize16(BaroAlt / 100);
serialize16(baro.BaroAlt / 100);
sendDataHead(ID_ALTITUDE_AP);
serialize16(ABS(BaroAlt % 100));
serialize16(ABS(baro.BaroAlt % 100));
}
#ifdef GPS
@ -212,7 +212,7 @@ static void sendTemperature1(void)
{
sendDataHead(ID_TEMPRATURE1);
#ifdef BARO
serialize16((baroTemperature + 50)/ 100); //Airmamaf
serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
#else
serialize16(telemTemperature1 / 10);
#endif

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

@ -654,7 +654,7 @@ void handleSmartPortTelemetry(void)
//case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE :
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;
}
break;
@ -700,15 +700,15 @@ void handleSmartPortTelemetry(void)
smartPortHasRequest = 0;
break;
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;
break;
case FSSP_DATAID_ACCY :
smartPortSendPackage(id, 100 * accSmooth[Y] / acc.acc_1G);
smartPortSendPackage(id, 100 * acc.accSmooth[Y] / acc.dev.acc_1G);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, 100 * accSmooth[Z] / acc.acc_1G);
smartPortSendPackage(id, 100 * acc.accSmooth[Z] / acc.dev.acc_1G);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_T1 :