From 089680ffce82a2b33c00c5a926fabec9dea19d46 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 28 Nov 2017 06:50:43 +0000 Subject: [PATCH] Scale acc value in acc sensor code --- src/main/blackbox/blackbox.c | 2 +- src/main/fc/fc_msp.c | 2 +- src/main/flight/imu.c | 25 ++++++++++---------- src/main/sensors/acceleration.c | 41 ++++++++++++++++++--------------- src/main/sensors/acceleration.h | 2 +- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/smartport.c | 6 ++--- 7 files changed, 41 insertions(+), 39 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 571dab357e..66e6d50f4a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1134,7 +1134,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->axisPID_I[i] = axisPID_I[i]; blackboxCurrent->axisPID_D[i] = axisPID_D[i]; blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); - blackboxCurrent->accADC[i] = acc.accADC[i]; + blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G); #ifdef MAG blackboxCurrent->magADC[i] = mag.magADC[i]; #endif diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6b8742d15c..65192d05b2 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -404,7 +404,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF // Hack scale due to choice of units for sensor data in multiwii const uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1; for (int i = 0; i < 3; i++) { - sbufWriteU16(dst, acc.accADC[i] / scale); + sbufWriteU16(dst, acc.accADCf[i] * acc.dev.acc_1G / scale); } for (int i = 0; i < 3; i++) { sbufWriteU16(dst, gyroRateDps(i)); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index e0d914d26d..f091e167fc 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -455,15 +455,14 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) static bool imuCanUseAccelerometerForCorrection(void) { - int32_t axis; - int32_t accMagnitudeSq = 0; + float accMagnitudeSq = 0; - for (axis = 0; axis < 3; axis++) { - accMagnitudeSq += (int32_t)acc.accADC[axis] * acc.accADC[axis]; + for (int axis = 0; axis < 3; axis++) { + accMagnitudeSq += acc.accADCf[axis] * acc.accADCf[axis]; } // Magnitude^2 in percent of G^2 - const int nearness = ABS(100 - (accMagnitudeSq * 100 / ((int32_t)acc.dev.acc_1G * acc.dev.acc_1G))); + const float nearness = ABS(100 - (accMagnitudeSq * 100)); return (nearness > MAX_ACC_SQ_NEARNESS) ? false : true; } @@ -568,7 +567,7 @@ static void imuUpdateMeasuredAcceleration(void) #else /* Convert acceleration to cm/s/s */ for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - imuMeasuredAccelBF.A[axis] = acc.accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G); + imuMeasuredAccelBF.A[axis] = acc.accADCf[axis] * GRAVITY_CMSS; } #endif } @@ -585,9 +584,9 @@ void imuHILUpdate(void) imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, attitude.values.yaw); /* Fake accADC readings */ - 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); + accADCf[X] = hilToFC.bodyAccel[X] / GRAVITY_CMSS; + accADCf[Y] = hilToFC.bodyAccel[Y] / GRAVITY_CMSS; + accADCf[Z] = hilToFC.bodyAccel[Z] / GRAVITY_CMSS; } #endif @@ -607,7 +606,7 @@ void imuUpdateAccelerometer(void) #ifdef ASYNC_GYRO_PROCESSING for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - imuAccumulatedAcc[axis] += acc.accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G); + imuAccumulatedAcc[axis] += acc.accADCf[axis] * GRAVITY_CMSS; } imuAccumulatedAccCount++; #endif @@ -637,9 +636,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) imuCalculateEstimatedAttitude(dT); // Update attitude estimate #endif } else { - acc.accADC[X] = 0; - acc.accADC[Y] = 0; - acc.accADC[Z] = 0; + acc.accADCf[X] = 0.0f; + acc.accADCf[Y] = 0.0f; + acc.accADCf[Z] = 0.0f; } } diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 2d806cc3dd..f0d16a3ada 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -69,6 +69,8 @@ FASTRAM acc_t acc; // acc access functions 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_FASTRAM int32_t accADC[XYZ_AXIS_COUNT]; + STATIC_FASTRAM biquadFilter_t accFilter[XYZ_AXIS_COUNT]; #ifdef USE_ACC_NOTCH @@ -376,7 +378,7 @@ int getPrimaryAxisIndex(int32_t sample[3]) static void performAcclerationCalibration(void) { - int axisIndex = getPrimaryAxisIndex(acc.accADC); + int axisIndex = getPrimaryAxisIndex(accADC); // Check if sample is usable if (axisIndex < 0) { @@ -398,10 +400,10 @@ static void performAcclerationCalibration(void) } if (!calibratedAxis[axisIndex]) { - sensorCalibrationPushSampleForOffsetCalculation(&calState, acc.accADC); - accSamples[axisIndex][X] += acc.accADC[X]; - accSamples[axisIndex][Y] += acc.accADC[Y]; - accSamples[axisIndex][Z] += acc.accADC[Z]; + sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC); + accSamples[axisIndex][X] += accADC[X]; + accSamples[axisIndex][Y] += accADC[Y]; + accSamples[axisIndex][Z] += accADC[Z]; if (isOnFinalAccelerationCalibrationCycle()) { calibratedAxis[axisIndex] = true; @@ -447,9 +449,9 @@ static void performAcclerationCalibration(void) static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain) { - acc.accADC[X] = (acc.accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096; - acc.accADC[Y] = (acc.accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096; - acc.accADC[Z] = (acc.accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096; + accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096; + accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096; + accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096; } void accUpdate(void) @@ -459,30 +461,31 @@ void accUpdate(void) } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accADC[axis] = acc.dev.ADCRaw[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; } if (accelerometerConfig()->acc_lpf_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis])); + acc.accADCf[axis] = biquadFilterApply(&accFilter[axis], acc.accADCf[axis]); } } #ifdef USE_ACC_NOTCH if (accelerometerConfig()->acc_notch_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accADC[axis] = lrintf(accNotchFilterApplyFn(accNotchFilter[axis], (float)acc.accADC[axis])); + acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]); } } #endif - - if (!accIsCalibrationComplete()) { - performAcclerationCalibration(); - } - - applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain); - - alignSensors(acc.accADC, acc.dev.accAlign); } void accSetCalibrationValues(void) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 9f1a58553f..5aec605054 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -41,7 +41,7 @@ typedef enum { typedef struct acc_s { accDev_t dev; uint32_t accTargetLooptime; - int32_t accADC[XYZ_AXIS_COUNT]; + float accADCf[XYZ_AXIS_COUNT]; // acceleration in g } acc_t; extern acc_t acc; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 8b4d751eea..19f53cc698 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -158,7 +158,7 @@ static void sendAccel(void) for (i = 0; i < 3; i++) { sendDataHead(ID_ACC_X + i); - serialize16(acc.accADC[i] * 1000 / acc.dev.acc_1G); + serialize16(lrintf(acc.accADCf[i] * 1000)); } } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index b750ce9c29..62edfb6d56 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -640,13 +640,13 @@ void handleSmartPortTelemetry(void) smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg break; case FSSP_DATAID_ACCX : - smartPortSendPackage(id, 100 * acc.accADC[X] / acc.dev.acc_1G); + smartPortSendPackage(id, lrintf(100 * acc.accADCf[X])); break; case FSSP_DATAID_ACCY : - smartPortSendPackage(id, 100 * acc.accADC[Y] / acc.dev.acc_1G); + smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y])); break; case FSSP_DATAID_ACCZ : - smartPortSendPackage(id, 100 * acc.accADC[Z] / acc.dev.acc_1G); + smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z])); break; case FSSP_DATAID_T1 : {