Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

ACC/GYRO: Cleanups. Thanks to @martinbudden for the idea and most of the code

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-05-09 10:50:47 +10:00
parent 1b1c446980
commit cf46cf6098
24 changed files with 78 additions and 91 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1240,7 +1240,7 @@ static bool blackboxWriteSysinfo()
blackboxPrintfHeaderLine("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
break;
case 8:
blackboxPrintfHeaderLine("acc_1G:%u", acc_1G);
blackboxPrintfHeaderLine("acc_1G:%u", acc.acc_1G);
break;
case 9:
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {

5
src/main/drivers/accgyro.h
View file

@ -17,8 +17,6 @@
#pragma once
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
@ -28,7 +26,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;

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

@ -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(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
i2cWrite(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;

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

@ -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(BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
acc_1G = 512 * 8;
acc->acc_1G = 512 * 8;
}
static bool bma280Read(int16_t *accelData)

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

@ -113,7 +113,7 @@ int32_t accelSummedSamples100Hz[3];
int32_t accelSummedSamples500Hz[3];
void lsm303dlhcAccInit(void)
void lsm303dlhcAccInit(acc_t *acc)
{
i2cWrite(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.

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

@ -77,7 +77,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)
@ -117,7 +117,7 @@ static inline void mma8451ConfigureInterrupt(void)
i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
}
static void mma8452Init(void)
static void mma8452Init(acc_t *acc)
{
i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
@ -129,7 +129,7 @@ static void mma8452Init(void)
i2cWrite(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)

8
src/main/drivers/accgyro_mpu6050.c
View file

@ -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 * 8;
acc->acc_1G = 256 * 8;
break;
case MPU_FULL_RESOLUTION:
acc_1G = 512 * 8;
acc->acc_1G = 512 * 8;
break;
}
}

6
src/main/drivers/accgyro_mpu6500.c
View file

@ -34,8 +34,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) {
@ -64,11 +62,11 @@ bool mpu6500GyroDetect(gyro_t *gyro)
return true;
}
void mpu6500AccInit(void)
void mpu6500AccInit(acc_t *acc)
{
mpuIntExtiInit();
acc_1G = 512 * 8;
acc->acc_1G = 512 * 8;
}
void mpu6500GyroInit(uint8_t lpf)

2
src/main/drivers/accgyro_mpu6500.h
View file

@ -24,5 +24,5 @@
bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro);
void mpu6500AccInit(void);
void mpu6500AccInit(acc_t *acc);
void mpu6500GyroInit(uint8_t lpf);

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

@ -144,11 +144,11 @@ void mpu6000SpiGyroInit(uint8_t lpf)
}
}
void mpu6000SpiAccInit(void)
void mpu6000SpiAccInit(acc_t *acc)
{
mpuIntExtiInit();
acc_1G = 512 * 8;
acc->acc_1G = 512 * 8;
}
bool mpu6000SpiDetect(void)

2
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -38,8 +38,6 @@
#define DISABLE_MPU6500 GPIO_SetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
#define ENABLE_MPU6500 GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
extern uint16_t acc_1G;
bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_MPU6500;

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

@ -34,7 +34,7 @@
extern gyro_t gyro;
uint32_t targetLooptime;
uint8_t mpuDividerDrops;
static uint8_t mpuDividerDrops;
bool getMpuDataStatus(gyro_t *gyro)
{
@ -44,15 +44,16 @@ bool getMpuDataStatus(gyro_t *gyro)
return mpuDataStatus;
}
bool gyroSyncCheckUpdate(void) {
bool gyroSyncCheckUpdate(void)
{
return getMpuDataStatus(&gyro);
}
void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator) {
int gyroSamplePeriod;
void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)
{
if (gyroSync) {
if (!lpf) {
int gyroSamplePeriod;
if (lpf == 0) {
gyroSamplePeriod = 125;
} else {
@ -60,13 +61,14 @@ void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint
}
mpuDividerDrops = gyroSyncDenominator - 1;
targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
} else {
mpuDividerDrops = 0;
targetLooptime = looptime;
}
}
uint8_t gyroMPU6xxxCalculateDivider(void) {
uint8_t gyroMPU6xxxCalculateDivider(void)
{
return mpuDividerDrops;
}

2
src/main/drivers/gyro_sync.h
View file

@ -21,4 +21,4 @@ extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxCalculateDivider(void);
void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);
void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);

2
src/main/drivers/sensor.h
View file

@ -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 (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor read and align prototype

10
src/main/flight/imu.c
View file

@ -389,7 +389,7 @@ static int imuCalculateAccelerometerConfidence(void)
}
// Magnitude^2 in percent of G^2
accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G);
accMagnitude = accMagnitude * 100 / ((int32_t)acc.acc_1G * acc.acc_1G);
int32_t nearness = ABS(100 - accMagnitude);
@ -454,7 +454,7 @@ static void imuUpdateMeasuredAcceleration(void)
/* Convert acceleration to cm/s/s */
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc_1G);
imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
}
@ -483,9 +483,9 @@ void imuHILUpdate(void)
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw);
/* Fake accADC readings */
accADC[X] = hilToFC.bodyAccel[X] * (acc_1G / GRAVITY_CMSS);
accADC[Y] = hilToFC.bodyAccel[Y] * (acc_1G / GRAVITY_CMSS);
accADC[Z] = hilToFC.bodyAccel[Z] * (acc_1G / GRAVITY_CMSS);
accADC[X] = hilToFC.bodyAccel[X] * (acc.acc_1G / GRAVITY_CMSS);
accADC[Y] = hilToFC.bodyAccel[Y] * (acc.acc_1G / GRAVITY_CMSS);
accADC[Z] = hilToFC.bodyAccel[Z] * (acc.acc_1G / GRAVITY_CMSS);
}
#endif

2
src/main/io/serial_msp.c
View file

@ -640,7 +640,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;
const uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1;
for (i = 0; i < 3; i++)
serialize16(accADC[i] / scale);

7
src/main/main.c
View file

@ -383,7 +383,6 @@ void init(void)
adcInit(&adc_params);
#endif
initBoardAlignment(&masterConfig.boardAlignment);
#ifdef DISPLAY
@ -398,9 +397,11 @@ void init(void)
}
#endif
// Set gyro sampling rate divider before initialization
gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator);
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf,
masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination,
masterConfig.looptime, masterConfig.gyroSync, masterConfig.gyroSyncDenominator)) {
masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);

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

@ -38,14 +38,12 @@
#include "sensors/acceleration.h"
int16_t accADCRaw[XYZ_AXIS_COUNT];
int32_t accADC[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
int32_t accADC[XYZ_AXIS_COUNT];
sensor_align_e accAlign = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static int16_t accADCRaw[XYZ_AXIS_COUNT];
static flightDynamicsTrims_t * accZero;
static flightDynamicsTrims_t * accGain;
@ -100,7 +98,6 @@ int getPrimaryAxisIndex(int32_t sample[3])
void performAcclerationCalibration(void)
{
int axisIndex = getPrimaryAxisIndex(accADC);
uint8_t axis;
// Check if sample is usable
if (axisIndex < 0) {
@ -109,7 +106,7 @@ void performAcclerationCalibration(void)
// Top-up and first calibration cycle, reset everything
if (axisIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
for (axis = 0; axis < 6; axis++) {
for (int axis = 0; axis < 6; axis++) {
calibratedAxis[axis] = false;
accSamples[axis][X] = 0;
accSamples[axis][Y] = 0;
@ -141,24 +138,24 @@ void performAcclerationCalibration(void)
/* Calculate offset */
sensorCalibrationSolveForOffset(&calState, accTmp);
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
accZero->raw[axis] = lrintf(accTmp[axis]);
}
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
sensorCalibrationResetState(&calState);
for (axis = 0; axis < 6; axis++) {
for (int axis = 0; axis < 6; axis++) {
accSample[X] = accSamples[axis][X] / CALIBRATING_ACC_CYCLES - accZero->raw[X];
accSample[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
accSample[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc_1G);
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.acc_1G);
}
sensorCalibrationSolveForScale(&calState, accTmp);
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
accGain->raw[axis] = lrintf(accTmp[axis] * 4096);
}
@ -177,18 +174,16 @@ void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_
void updateAccelerationReadings(void)
{
int axis;
if (!acc.read(accADCRaw)) {
return;
}
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
filterInitBiQuad(accLpfCutHz, &accFilterState[axis], 0);
}
@ -197,7 +192,7 @@ void updateAccelerationReadings(void)
}
if (accFilterInitialised) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = lrintf(filterApplyBiQuad((float) accADC[axis], &accFilterState[axis]));
}
}

1
src/main/sensors/acceleration.h
View file

@ -35,7 +35,6 @@ typedef enum {
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint16_t acc_1G;
extern int32_t accADC[XYZ_AXIS_COUNT];

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

@ -36,20 +36,21 @@
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
uint16_t calibratingG = 0;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static uint16_t calibratingG = 0;
static int16_t gyroADCRaw[XYZ_AXIS_COUNT];
static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static int8_t gyroLpfCutHz = 0;
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
static bool gyroFilterInitialised = false;
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
{
gyroConfig = gyroConfigToUse;
@ -66,23 +67,22 @@ bool isGyroCalibrationComplete(void)
return calibratingG == 0;
}
bool isOnFinalGyroCalibrationCycle(void)
static bool isOnFinalGyroCalibrationCycle(void)
{
return calibratingG == 1;
}
bool isOnFirstGyroCalibrationCycle(void)
static bool isOnFirstGyroCalibrationCycle(void)
{
return calibratingG == CALIBRATING_GYRO_CYCLES;
}
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
{
int8_t axis;
static int32_t g[3];
static stdev_t var[3];
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle()) {
@ -118,28 +118,25 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
static void applyGyroZero(void)
{
int8_t axis;
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
}
}
void gyroUpdate(void)
{
int8_t axis;
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
// Prepare a copy of int32_t gyroADC for mangling to prevent overflow
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis];
if (gyroLpfCutHz) {
if (!gyroFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
}
@ -148,7 +145,7 @@ void gyroUpdate(void)
}
if (gyroFilterInitialised) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
}
}

1
src/main/sensors/gyro.h
View file

@ -33,7 +33,6 @@ extern gyro_t gyro;
extern sensor_align_e gyroAlign;
extern int32_t gyroADC[XYZ_AXIS_COUNT];
extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
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.

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

@ -745,8 +745,7 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig,
uint32_t looptime, uint8_t gyroSync, uint8_t gyroSyncDenominator) {
int16_t magDeclinationFromConfig) {
int16_t deg, min;
@ -770,9 +769,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t g
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyroUpdateSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator); // Set gyro sampling rate divider before initialization
acc.init(&acc);
gyro.init(gyroLpf);
detectMag(magHardwareToUse);

3
src/main/sensors/initialisation.h
View file

@ -19,5 +19,4 @@
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf,
uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig,
uint32_t looptime, uint8_t gyroSync, uint8_t gyroSyncDenominator);
int16_t magDeclinationFromConfig);

2
src/main/telemetry/frsky.c
View file

@ -166,7 +166,7 @@ static void sendAccel(void)
for (i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i);
serialize16(((float)accADC[i] / acc_1G) * 1000);
serialize16(accADC[i] * 1000 / acc.acc_1G);
}
}