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

Fix integer division in accelerometer scaling to 1G; Optimize accumulated acceleration calculation

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-11-29 12:25:29 +10:00
parent 70bde5d51c
commit f81d5b2f4d
3 changed files with 20 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -539,10 +539,6 @@ void imuUpdateAccelerometer(void)
isAccelUpdatedAtLeastOnce = true; isAccelUpdatedAtLeastOnce = true;
} }
#endif #endif
#ifdef ASYNC_GYRO_PROCESSING
accUpdateAccumulatedMeasurements();
#endif
} }
void imuUpdateAttitude(timeUs_t currentTimeUs) void imuUpdateAttitude(timeUs_t currentTimeUs)

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

@ -458,7 +458,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
STATIC_FASTRAM float accumulatedMeasurements[XYZ_AXIS_COUNT]; STATIC_FASTRAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
STATIC_FASTRAM int accumulatedMeasurementCount; STATIC_FASTRAM int accumulatedMeasurementCount;
void accUpdateAccumulatedMeasurements(void) static void accUpdateAccumulatedMeasurements(void)
{ {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulatedMeasurements[axis] += acc.accADCf[axis]; accumulatedMeasurements[axis] += acc.accADCf[axis];
@ -473,11 +473,18 @@ void accUpdateAccumulatedMeasurements(void)
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc) void accGetMeasuredAcceleration(t_fp_vector *measuredAcc)
{ {
#ifdef ASYNC_GYRO_PROCESSING #ifdef ASYNC_GYRO_PROCESSING
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (accumulatedMeasurementCount) {
measuredAcc->A[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulatedMeasurements[axis] = 0; measuredAcc->A[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount;
accumulatedMeasurements[axis] = 0;
}
accumulatedMeasurementCount = 0;
}
else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
}
} }
accumulatedMeasurementCount = 0;;
#else #else
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS; measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
@ -494,14 +501,17 @@ void accUpdate(void)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = acc.dev.ADCRaw[axis]; accADC[axis] = acc.dev.ADCRaw[axis];
} }
if (!accIsCalibrationComplete()) { if (!accIsCalibrationComplete()) {
performAcclerationCalibration(); performAcclerationCalibration();
return; return;
} }
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain); applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
alignSensors(accADC, acc.dev.accAlign); alignSensors(accADC, acc.dev.accAlign);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accADC[axis] / acc.dev.acc_1G; acc.accADCf[axis] = (float)accADC[axis] / acc.dev.acc_1G;
} }
if (accelerometerConfig()->acc_lpf_hz) { if (accelerometerConfig()->acc_lpf_hz) {
@ -517,6 +527,10 @@ void accUpdate(void)
} }
} }
#endif #endif
#ifdef ASYNC_GYRO_PROCESSING
accUpdateAccumulatedMeasurements();
#endif
} }
void accSetCalibrationValues(void) void accSetCalibrationValues(void)

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

@ -65,7 +65,6 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(uint32_t accTargetLooptime); bool accInit(uint32_t accTargetLooptime);
bool accIsCalibrationComplete(void); bool accIsCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void accUpdateAccumulatedMeasurements(void);
void accGetMeasuredAcceleration(t_fp_vector *measuredAcc); void accGetMeasuredAcceleration(t_fp_vector *measuredAcc);
void accUpdate(void); void accUpdate(void);
void accSetCalibrationValues(void); void accSetCalibrationValues(void);