diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 80df3ba141..32d7b8e260 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1146,7 +1146,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); - BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc_1G); + BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.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 1a99d98381..385ea90b97 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -21,8 +21,6 @@ #define MPU_I2C_INSTANCE I2C_DEVICE #endif -extern uint16_t acc_1G; // FIXME move into acc_t - typedef struct gyro_s { sensorGyroInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function @@ -32,7 +30,8 @@ typedef struct gyro_s { } gyro_t; typedef struct acc_s { - sensorInitFuncPtr init; // initialize function + 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; diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c index 57b5ea339a..5604042778 100644 --- a/src/main/drivers/accgyro_adxl345.c +++ b/src/main/drivers/accgyro_adxl345.c @@ -56,7 +56,7 @@ #define ADXL345_RANGE_16G 0x03 #define ADXL345_FIFO_STREAM 0x80 -static void adxl345Init(void); +static void adxl345Init(acc_t *acc); static bool adxl345Read(int16_t *accelData); static bool useFifo = false; @@ -78,7 +78,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc) return true; } -static void adxl345Init(void) +static void adxl345Init(acc_t *acc) { if (useFifo) { uint8_t fifoDepth = 16; @@ -91,7 +91,7 @@ static void adxl345Init(void) i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); } - acc_1G = 265; // 3.3V operation // FIXME verify this is supposed to be 265, not 256. Typo? + acc->acc_1G = 256; // 3.3V operation } uint8_t acc_samples = 0; diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c index b93fda695d..866d944e3d 100644 --- a/src/main/drivers/accgyro_bma280.c +++ b/src/main/drivers/accgyro_bma280.c @@ -32,7 +32,7 @@ #define BMA280_PMU_BW 0x10 #define BMA280_PMU_RANGE 0x0F -static void bma280Init(void); +static void bma280Init(acc_t *acc); static bool bma280Read(int16_t *accelData); bool bma280Detect(acc_t *acc) @@ -49,12 +49,12 @@ bool bma280Detect(acc_t *acc) return true; } -static void bma280Init(void) +static void bma280Init(acc_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 - acc_1G = 512 * 8; + acc->acc_1G = 512 * 8; } static bool bma280Read(int16_t *accelData) diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index aa47f258c0..505764b29a 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -113,7 +113,7 @@ int32_t accelSummedSamples100Hz[3]; int32_t accelSummedSamples500Hz[3]; -void lsm303dlhcAccInit(void) +void lsm303dlhcAccInit(acc_t *acc) { i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); @@ -127,7 +127,7 @@ void lsm303dlhcAccInit(void) delay(100); - acc_1G = 512 * 8; + acc->acc_1G = 512 * 8; } // Read 3 gyro values into user-provided buffer. No overrun checking is done. diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index ad22c64f0a..2687c34b1a 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -81,7 +81,7 @@ static uint8_t device_id; -static void mma8452Init(void); +static void mma8452Init(acc_t *acc); static bool mma8452Read(int16_t *accelData); bool mma8452Detect(acc_t *acc) @@ -114,7 +114,7 @@ static inline void mma8451ConfigureInterrupt(void) i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 } -static void mma8452Init(void) +static void mma8452Init(acc_t *acc) { i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff @@ -126,7 +126,7 @@ static void mma8452Init(void) i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G. - acc_1G = 256; + acc->acc_1G = 256; } static bool mma8452Read(int16_t *accelData) diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 6fca12e059..d2717b1350 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -51,7 +51,7 @@ extern uint8_t mpuLowPassFilter; #define MPU6050_SMPLRT_DIV 0 // 8000Hz -static void mpu6050AccInit(void); +static void mpu6050AccInit(acc_t *acc); static void mpu6050GyroInit(uint8_t lpf); bool mpu6050AccDetect(acc_t *acc) @@ -82,16 +82,16 @@ bool mpu6050GyroDetect(gyro_t *gyro) return true; } -static void mpu6050AccInit(void) +static void mpu6050AccInit(acc_t *acc) { mpuIntExtiInit(); switch (mpuDetectionResult.resolution) { case MPU_HALF_RESOLUTION: - acc_1G = 256 * 4; + acc->acc_1G = 256 * 4; break; case MPU_FULL_RESOLUTION: - acc_1G = 512 * 4; + acc->acc_1G = 512 * 4; break; } } diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index d157984ec8..0cc23cfa9b 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -35,8 +35,6 @@ #include "accgyro_mpu.h" #include "accgyro_mpu6500.h" -extern uint16_t acc_1G; - bool mpu6500AccDetect(acc_t *acc) { if (mpuDetectionResult.sensor != MPU_65xx_I2C) { @@ -65,11 +63,11 @@ bool mpu6500GyroDetect(gyro_t *gyro) return true; } -void mpu6500AccInit(void) +void mpu6500AccInit(acc_t *acc) { mpuIntExtiInit(); - acc_1G = 512 * 4; + acc->acc_1G = 512 * 4; } void mpu6500GyroInit(uint8_t lpf) diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index 3c2339e32d..ecdd100bc0 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -26,5 +26,5 @@ bool mpu6500AccDetect(acc_t *acc); bool mpu6500GyroDetect(gyro_t *gyro); -void mpu6500AccInit(void); +void mpu6500AccInit(acc_t *acc); void mpu6500GyroInit(uint8_t lpf); diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 31c7ad6fb1..56143f0e4e 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -144,11 +144,11 @@ void mpu6000SpiGyroInit(uint8_t lpf) } } -void mpu6000SpiAccInit(void) +void mpu6000SpiAccInit(acc_t *acc) { mpuIntExtiInit(); - acc_1G = 512 * 4; + acc->acc_1G = 512 * 4; } bool mpu6000SpiDetect(void) diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 3cc8ab1ed9..316a296c3a 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -40,8 +40,6 @@ static IO_t mpuSpi6500CsPin = IO_NONE; -extern uint16_t acc_1G; - bool mpu6500WriteRegister(uint8_t reg, uint8_t data) { ENABLE_MPU6500; diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 72248c30a9..2721d05594 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -116,11 +116,11 @@ void mpu9250SpiGyroInit(uint8_t lpf) } } -void mpu9250SpiAccInit(void) +void mpu9250SpiAccInit(acc_t *acc) { mpuIntExtiInit(); - acc_1G = 512 * 8; + acc->acc_1G = 512 * 8; } diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 739e3e4b5b..6554f8267d 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -17,7 +17,9 @@ #pragma once +struct acc_s; typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype -typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype +typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype +typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 0906c4ac47..2e200cff1f 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -124,7 +124,7 @@ void imuInit(void) { smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)); gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second - accVelScale = 9.80665f / acc_1G / 10000.0f; + accVelScale = 9.80665f / acc.acc_1G / 10000.0f; imuComputeRotationMatrix(); } @@ -189,7 +189,7 @@ void imuCalculateAcceleration(uint32_t deltaT) } accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis } else - accel_ned.V.Z -= acc_1G; + accel_ned.V.Z -= acc.acc_1G; accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter @@ -362,7 +362,7 @@ static bool imuIsAccelerometerHealthy(void) accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis]; } - accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G)); + accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.acc_1G)); // Accept accel readings only in range 0.90g - 1.10g return (81 < accMagnitude) && (accMagnitude < 121); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index df3988e281..b332c1373b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -764,7 +764,7 @@ static bool processOutCommand(uint8_t cmdMSP) headSerialReply(18); // Hack scale due to choice of units for sensor data in multiwii - uint8_t scale = (acc_1G > 1024) ? 8 : 1; + uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1; for (i = 0; i < 3; i++) serialize16(accSmooth[i] / scale); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 5631827625..320d00ddde 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -43,7 +43,6 @@ int32_t accSmooth[XYZ_AXIS_COUNT]; acc_t acc; // acc access functions sensor_align_e accAlign = 0; -uint16_t acc_1G = 256; // this is the 1G measured acceleration. uint32_t accTargetLooptime; 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. @@ -109,7 +108,7 @@ 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_1G; + accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.acc_1G; resetRollAndPitchTrims(rollAndPitchTrims); @@ -164,7 +163,7 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri AccInflightCalibrationSavetoEEProm = false; accelerationTrims->raw[X] = b[X] / 50; accelerationTrims->raw[Y] = b[Y] / 50; - accelerationTrims->raw[Z] = b[Z] / 50 - acc_1G; // for nunchuck 200=1G + accelerationTrims->raw[Z] = b[Z] / 50 - acc.acc_1G; // for nunchuck 200=1G resetRollAndPitchTrims(rollAndPitchTrims); diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 38006a04f9..9e56c2abea 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -35,7 +35,6 @@ typedef enum { extern sensor_align_e accAlign; extern acc_t acc; -extern uint16_t acc_1G; extern uint32_t accTargetLooptime; extern int32_t accSmooth[XYZ_AXIS_COUNT]; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index e856112948..a69a976b67 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -627,8 +627,10 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a // Now time to init things, acc first - if (sensors(SENSOR_ACC)) - acc.init(); + if (sensors(SENSOR_ACC)) { + acc.acc_1G = 256; // set default + acc.init(&acc); + } // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation gyro.init(gyroLpf); diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 25638fb546..ec7122ddea 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -606,7 +606,7 @@ 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_1G > 1024) ? 8 : 1; + uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1; for (i = 0; i < 3; i++) bstWrite16(accSmooth[i] / scale); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 9e59d2709c..35185b9676 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -166,7 +166,7 @@ static void sendAccel(void) for (i = 0; i < 3; i++) { sendDataHead(ID_ACC_X + i); - serialize16(((float)accSmooth[i] / acc_1G) * 1000); + serialize16(((float)accSmooth[i] / acc.acc_1G) * 1000); } } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c575a74f59..791e40d62c 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -79,7 +79,7 @@ uint32_t rcModeActivationMask; int16_t rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; -uint16_t acc_1G; +acc_t acc; int16_t heading; gyro_t gyro; int16_t magADC[XYZ_AXIS_COUNT];