diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b5b5b5e7e..fe3ff4efe 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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)) { diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index f6f3a7718..372273ef5 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -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; diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c index b2603f3e9..848b04a7a 100644 --- a/src/main/drivers/accgyro_adxl345.c +++ b/src/main/drivers/accgyro_adxl345.c @@ -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; diff --git a/src/main/drivers/accgyro_adxl345.h b/src/main/drivers/accgyro_adxl345.h index 654b0e429..ee664fc86 100644 --- a/src/main/drivers/accgyro_adxl345.h +++ b/src/main/drivers/accgyro_adxl345.h @@ -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); diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c index cf543dfd9..3b0ec6e85 100644 --- a/src/main/drivers/accgyro_bma280.c +++ b/src/main/drivers/accgyro_bma280.c @@ -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 diff --git a/src/main/drivers/accgyro_bma280.h b/src/main/drivers/accgyro_bma280.h index 8ecc1a4a8..b4624d66a 100644 --- a/src/main/drivers/accgyro_bma280.h +++ b/src/main/drivers/accgyro_bma280.h @@ -17,4 +17,4 @@ #pragma once -bool bma280Detect(acc_t *acc); +bool bma280Detect(accDev_t *acc); diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c index 26e58f44b..2183bebdf 100644 --- a/src/main/drivers/accgyro_l3g4200d.c +++ b/src/main/drivers/accgyro_l3g4200d.c @@ -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]; diff --git a/src/main/drivers/accgyro_l3g4200d.h b/src/main/drivers/accgyro_l3g4200d.h index 40c05f6e4..c4366f227 100644 --- a/src/main/drivers/accgyro_l3g4200d.h +++ b/src/main/drivers/accgyro_l3g4200d.h @@ -17,4 +17,4 @@ #pragma once -bool l3g4200dDetect(gyro_t *gyro); +bool l3g4200dDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index 7432ead75..12f83e519 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -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; diff --git a/src/main/drivers/accgyro_l3gd20.h b/src/main/drivers/accgyro_l3gd20.h index 11b1241e9..db661718e 100644 --- a/src/main/drivers/accgyro_l3gd20.h +++ b/src/main/drivers/accgyro_l3gd20.h @@ -17,4 +17,4 @@ #pragma once -bool l3gd20Detect(gyro_t *gyro); +bool l3gd20Detect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 43b75d585..71baf89f1 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -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; diff --git a/src/main/drivers/accgyro_lsm303dlhc.h b/src/main/drivers/accgyro_lsm303dlhc.h index 1f5c56f68..08e85ff44 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.h +++ b/src/main/drivers/accgyro_lsm303dlhc.h @@ -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); diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index a21507ece..c4b65fdcc 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -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 diff --git a/src/main/drivers/accgyro_mma845x.h b/src/main/drivers/accgyro_mma845x.h index cdfdba727..bb8887ccf 100644 --- a/src/main/drivers/accgyro_mma845x.h +++ b/src/main/drivers/accgyro_mma845x.h @@ -17,4 +17,4 @@ #pragma once -bool mma8452Detect(acc_t *acc); +bool mma8452Detect(accDev_t *acc); diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 88ceca8b7..3f59e2fb9 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -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) { diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 125fbfb27..2eafe89f0 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -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); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index 661b576d8..4d8d84fba 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -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; diff --git a/src/main/drivers/accgyro_mpu3050.h b/src/main/drivers/accgyro_mpu3050.h index fd4c28e10..dbbe18ee6 100644 --- a/src/main/drivers/accgyro_mpu3050.h +++ b/src/main/drivers/accgyro_mpu3050.h @@ -26,4 +26,4 @@ #define MPU3050_USER_CTRL 0x3D #define MPU3050_PWR_MGM 0x3E -bool mpu3050Detect(gyro_t *gyro); +bool mpu3050Detect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 4cfa8e375..af91df4e8 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -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); diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h index d8649a3c6..eceb27986 100644 --- a/src/main/drivers/accgyro_mpu6050.h +++ b/src/main/drivers/accgyro_mpu6050.h @@ -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); diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index ade07baf7..d4257c15f 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -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); diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index 06f6e13c6..3cedd2027 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -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); diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index a73c7204d..95d94fccc 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -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); diff --git a/src/main/drivers/accgyro_spi_icm20689.h b/src/main/drivers/accgyro_spi_icm20689.h index 60f9bc945..bb9451ef4 100644 --- a/src/main/drivers/accgyro_spi_icm20689.h +++ b/src/main/drivers/accgyro_spi_icm20689.h @@ -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); diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 7bf631928..2f0fe9443 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -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; diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h index 0081bddeb..ccd22438d 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.h +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -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); diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index fb1f52229..b14a9ca93 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -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; diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index 969b913bc..b5c2758fc 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -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); diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 0fd502f64..81415fac1 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -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; diff --git a/src/main/drivers/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro_spi_mpu9250.h index 1af293288..362ecd2e3 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro_spi_mpu9250.h @@ -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); diff --git a/src/main/drivers/barometer.h b/src/main/drivers/barometer.h index dec11ad3c..1c63631bf 100644 --- a/src/main/drivers/barometer.h +++ b/src/main/drivers/barometer.h @@ -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 diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index 534d4b424..50862ebfc 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -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; diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index ffe863d1b..f6c6dece7 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -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) diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c index bebf45ac3..b034eac3d 100644 --- a/src/main/drivers/barometer_bmp280.c +++ b/src/main/drivers/barometer_bmp280.c @@ -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; diff --git a/src/main/drivers/barometer_bmp280.h b/src/main/drivers/barometer_bmp280.h index b0a4829fb..ee8368b36 100644 --- a/src/main/drivers/barometer_bmp280.h +++ b/src/main/drivers/barometer_bmp280.h @@ -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); diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c index 0820d89af..8c0d7c27d 100644 --- a/src/main/drivers/barometer_ms5611.c +++ b/src/main/drivers/barometer_ms5611.c @@ -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; diff --git a/src/main/drivers/barometer_ms5611.h b/src/main/drivers/barometer_ms5611.h index c9f90c46d..e1ad78763 100644 --- a/src/main/drivers/barometer_ms5611.h +++ b/src/main/drivers/barometer_ms5611.h @@ -17,4 +17,4 @@ #pragma once -bool ms5611Detect(baro_t *baro); +bool ms5611Detect(baroDev_t *baro); diff --git a/src/main/drivers/compass.h b/src/main/drivers/compass.h index 1d2c88346..019e65417 100644 --- a/src/main/drivers/compass.h +++ b/src/main/drivers/compass.h @@ -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 diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 0101dbe6f..b90abf1bd 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -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; diff --git a/src/main/drivers/compass_ak8963.h b/src/main/drivers/compass_ak8963.h index 9806a1d1c..34a492225 100644 --- a/src/main/drivers/compass_ak8963.h +++ b/src/main/drivers/compass_ak8963.h @@ -17,6 +17,6 @@ #pragma once -bool ak8963Detect(mag_t *mag); +bool ak8963Detect(magDev_t *mag); void ak8963Init(void); bool ak8963Read(int16_t *magData); diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index b0046d3b6..b86218f8a 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -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); diff --git a/src/main/drivers/compass_ak8975.h b/src/main/drivers/compass_ak8975.h index 493114cb3..5a7d729be 100644 --- a/src/main/drivers/compass_ak8975.h +++ b/src/main/drivers/compass_ak8975.h @@ -17,6 +17,6 @@ #pragma once -bool ak8975Detect(mag_t *mag); +bool ak8975Detect(magDev_t *mag); void ak8975Init(void); bool ak8975Read(int16_t *magData); diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 72794f8dd..67dc75480 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -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; diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index e2f71fd28..215bc84af 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -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); diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 5d60aaffd..c1df19077 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -16,7 +16,7 @@ static uint8_t mpuDividerDrops; -bool gyroSyncCheckUpdate(gyro_t *gyro) +bool gyroSyncCheckUpdate(gyroDev_t *gyro) { if (!gyro->intStatus) return false; diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index ca5111aa1..1b0787894 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -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); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 1cbe7725d..3e897f8c1 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 710ab3e59..85e58fd0b 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index a94666b37..fc93fb98f 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -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)); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 0d350a9f7..82e2d5423 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -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 diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 9907f3b52..a04bd00ac 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -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 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f733862cf..d6c898c00 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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; } } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6aa67f4e6..80bbf9b2f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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). ---------- diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 368c4526d..11bdf2ef1 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -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); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 329415234..e0afc4e7b 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d2a85c239..4a5b6cfe8 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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); } } } diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 04b6ee19f..a528524bc 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -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; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 1e55b3dd9..11ba145e8 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -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); } } } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 07e608e9a..07be4da77 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -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; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 9e428ad7e..7534efaf8 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -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) diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 0ef2638ad..f68465251 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -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); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 7354be73f..e7e500c49 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -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; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 13abb7509..f64c517c2 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6dfb8a4b0..3358ec11e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index df2426e42..3034dd5a0 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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. diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index b46787ef0..97fd00754 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -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;iinit = 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 diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 63351f0a6..889ebe09e 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -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 diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index df2f812ba..c2a5c0e5d 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -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 diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 48c81bb9e..93f7ef458 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -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 :