diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f4b27b18cc..cda9d20504 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -539,10 +539,6 @@ void imuUpdateAccelerometer(void) isAccelUpdatedAtLeastOnce = true; } #endif - -#ifdef ASYNC_GYRO_PROCESSING - accUpdateAccumulatedMeasurements(); -#endif } void imuUpdateAttitude(timeUs_t currentTimeUs) diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 5fae52e51b..4a1aba0a8d 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -458,7 +458,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f STATIC_FASTRAM float accumulatedMeasurements[XYZ_AXIS_COUNT]; STATIC_FASTRAM int accumulatedMeasurementCount; -void accUpdateAccumulatedMeasurements(void) +static void accUpdateAccumulatedMeasurements(void) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { accumulatedMeasurements[axis] += acc.accADCf[axis]; @@ -473,11 +473,18 @@ void accUpdateAccumulatedMeasurements(void) void accGetMeasuredAcceleration(t_fp_vector *measuredAcc) { #ifdef ASYNC_GYRO_PROCESSING - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - measuredAcc->A[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount; - accumulatedMeasurements[axis] = 0; + if (accumulatedMeasurementCount) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + 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 for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS; @@ -494,14 +501,17 @@ void accUpdate(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { accADC[axis] = acc.dev.ADCRaw[axis]; } + if (!accIsCalibrationComplete()) { performAcclerationCalibration(); return; } + applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain); alignSensors(accADC, acc.dev.accAlign); + 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) { @@ -517,6 +527,10 @@ void accUpdate(void) } } #endif + +#ifdef ASYNC_GYRO_PROCESSING + accUpdateAccumulatedMeasurements(); +#endif } void accSetCalibrationValues(void) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 2b1f9be4c2..96f7a59ee7 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -65,7 +65,6 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig); bool accInit(uint32_t accTargetLooptime); bool accIsCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); -void accUpdateAccumulatedMeasurements(void); void accGetMeasuredAcceleration(t_fp_vector *measuredAcc); void accUpdate(void); void accSetCalibrationValues(void);