diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index df798c32e1..f241d7b818 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -289,7 +289,7 @@ typedef struct blackboxMainState_s { int16_t rcCommand[4]; int16_t gyroADC[XYZ_AXIS_COUNT]; - int16_t accSmooth[XYZ_AXIS_COUNT]; + int16_t accADC[XYZ_AXIS_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT]; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t servo[MAX_SUPPORTED_SERVOS]; @@ -580,7 +580,7 @@ static void writeIntraframe(void) blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { - blackboxWriteSigned16VBArray(blackboxCurrent->accSmooth, XYZ_AXIS_COUNT); + blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) { @@ -713,7 +713,7 @@ static void writeInterframe(void) //Since gyros, accs and motors are noisy, base their predictions on the average of the history: blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT); if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { - blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT); + blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT); } if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) { blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT); @@ -994,7 +994,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->accSmooth[i] = acc.accSmooth[i]; + blackboxCurrent->accADC[i] = acc.accADC[i]; #ifdef USE_MAG blackboxCurrent->magADC[i] = mag.magADC[i]; #endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4e9117f67f..7591f98898 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -134,7 +134,7 @@ static bool isCalibrating(void) // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG - return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); + return (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } void resetArmingDisabled(void) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 396a8a9d41..86a44cb1c4 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -200,9 +200,9 @@ static void imuCalculateAcceleration(uint32_t deltaT) const float dT = (float)deltaT * 1e-6f; t_fp_vector accel_ned; - accel_ned.V.X = acc.accSmooth[X]; - accel_ned.V.Y = acc.accSmooth[Y]; - accel_ned.V.Z = acc.accSmooth[Z]; + accel_ned.V.X = acc.accADC[X]; + accel_ned.V.Y = acc.accADC[Y]; + accel_ned.V.Z = acc.accADC[Z]; imuTransformVectorBodyToEarth(&accel_ned); @@ -392,7 +392,7 @@ static bool imuIsAccelerometerHealthy(void) { float accMagnitude = 0; for (int axis = 0; axis < 3; axis++) { - const float a = acc.accSmooth[axis]; + const float a = acc.accADC[axis]; accMagnitude += a * a; } @@ -478,9 +478,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) imuCalculateEstimatedAttitude(currentTimeUs); IMU_UNLOCK; } else { - acc.accSmooth[X] = 0; - acc.accSmooth[Y] = 0; - acc.accSmooth[Z] = 0; + acc.accADC[X] = 0; + acc.accADC[Y] = 0; + acc.accADC[Z] = 0; } } diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index f2b5cc351f..e064a166c4 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -779,7 +779,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) } for (int i = 0; i < 3; i++) { - sbufWriteU16(dst, acc.accSmooth[i] / scale); + sbufWriteU16(dst, acc.accADC[i] / scale); } for (int i = 0; i < 3; i++) { sbufWriteU16(dst, gyroRateDps(i)); diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 3a98ec4cf5..efbe867869 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -476,7 +476,7 @@ static void showSensorsPage(void) i2c_OLED_send_string(bus, " X Y Z"); if (sensors(SENSOR_ACC)) { - tfp_sprintf(lineBuffer, format, "ACC", acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z]); + tfp_sprintf(lineBuffer, format, "ACC", acc.accADC[X], acc.accADC[Y], acc.accADC[Z]); padLineBuffer(); i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_send_string(bus, lineBuffer); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 736ef86610..b0deb47927 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -231,11 +231,10 @@ retry: case ACC_ICM20608G: #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #ifdef USE_ACC_SPI_MPU6500 - if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) + if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) { #else - if (mpu6500AccDetect(dev)) + if (mpu6500AccDetect(dev)) { #endif - { #ifdef ACC_MPU6500_ALIGN dev->accAlign = ACC_MPU6500_ALIGN; #endif @@ -371,7 +370,7 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) calibratingA = calibrationCyclesRequired; } -bool isAccelerationCalibrationComplete(void) +bool accIsCalibrationComplete(void) { return calibratingA == 0; } @@ -393,14 +392,15 @@ static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims for (int axis = 0; axis < 3; axis++) { // Reset a[axis] at start of calibration - if (isOnFirstAccelerationCalibrationCycle()) + if (isOnFirstAccelerationCalibrationCycle()) { a[axis] = 0; + } // Sum up CALIBRATING_ACC_CYCLES readings - a[axis] += acc.accSmooth[axis]; + a[axis] += acc.accADC[axis]; // Reset global variables to prevent other code from using un-calibrated data - acc.accSmooth[axis] = 0; + acc.accADC[axis] = 0; accelerationTrims->raw[axis] = 0; } @@ -420,7 +420,6 @@ static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { - uint8_t axis; static int32_t b[3]; static int16_t accZero_saved[3] = { 0, 0, 0 }; static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } }; @@ -434,14 +433,14 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch; } if (InflightcalibratingA > 0) { - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { // Reset a[axis] at start of calibration if (InflightcalibratingA == 50) b[axis] = 0; // Sum up 50 readings - b[axis] += acc.accSmooth[axis]; + b[axis] += acc.accADC[axis]; // Clear global variables for next reading - acc.accSmooth[axis] = 0; + acc.accADC[axis] = 0; accelerationTrims->raw[axis] = 0; } // all values are measured @@ -473,9 +472,9 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims) { - acc.accSmooth[X] -= accelerationTrims->raw[X]; - acc.accSmooth[Y] -= accelerationTrims->raw[Y]; - acc.accSmooth[Z] -= accelerationTrims->raw[Z]; + acc.accADC[X] -= accelerationTrims->raw[X]; + acc.accADC[Y] -= accelerationTrims->raw[Y]; + acc.accADC[Z] -= accelerationTrims->raw[Z]; } void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) @@ -489,18 +488,18 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[axis]); - acc.accSmooth[axis] = acc.dev.ADCRaw[axis]; + acc.accADC[axis] = acc.dev.ADCRaw[axis]; } if (accLpfCutHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accSmooth[axis])); + acc.accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis])); } } - alignSensors(acc.accSmooth, acc.dev.accAlign); + alignSensors(acc.accADC, acc.dev.accAlign); - if (!isAccelerationCalibrationComplete()) { + if (!accIsCalibrationComplete()) { performAcclerationCalibration(rollAndPitchTrims); } @@ -512,7 +511,7 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) ++accumulatedMeasurementCount; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accumulatedMeasurements[axis] += acc.accSmooth[axis]; + accumulatedMeasurements[axis] += acc.accADC[axis]; } } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index d00ff359e8..604542f0ff 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -46,7 +46,7 @@ typedef enum { typedef struct acc_s { accDev_t dev; uint32_t accSamplingInterval; - int32_t accSmooth[XYZ_AXIS_COUNT]; + int32_t accADC[XYZ_AXIS_COUNT]; bool isAccelUpdatedAtLeastOnce; } acc_t; @@ -75,7 +75,7 @@ typedef struct accelerometerConfig_s { PG_DECLARE(accelerometerConfig_t, accelerometerConfig); bool accInit(uint32_t gyroTargetLooptime); -bool isAccelerationCalibrationComplete(void); +bool accIsCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims); diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 9abd396859..f53fd8be34 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -173,7 +173,7 @@ static void frSkyHubWriteByteInternal(const char data) static void sendAccel(void) { for (unsigned i = 0; i < 3; i++) { - frSkyHubWriteFrame(ID_ACC_X + i, ((float)acc.accSmooth[i] / acc.dev.acc_1G) * 1000); + frSkyHubWriteFrame(ID_ACC_X + i, ((float)acc.accADC[i] / acc.dev.acc_1G) * 1000); } } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 3290f6fc25..237254886a 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -418,15 +418,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear *clearToSend = false; break; case FSSP_DATAID_ACCX : - smartPortSendPackage(id, 100 * acc.accSmooth[X] / acc.dev.acc_1G); // Multiply by 100 to show as x.xx g on Taranis + smartPortSendPackage(id, 100 * acc.accADC[X] / acc.dev.acc_1G); // Multiply by 100 to show as x.xx g on Taranis *clearToSend = false; break; case FSSP_DATAID_ACCY : - smartPortSendPackage(id, 100 * acc.accSmooth[Y] / acc.dev.acc_1G); + smartPortSendPackage(id, 100 * acc.accADC[Y] / acc.dev.acc_1G); *clearToSend = false; break; case FSSP_DATAID_ACCZ : - smartPortSendPackage(id, 100 * acc.accSmooth[Z] / acc.dev.acc_1G); + smartPortSendPackage(id, 100 * acc.accADC[Z] / acc.dev.acc_1G); *clearToSend = false; break; case FSSP_DATAID_T1 : diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 8eb233b8ff..c411baa656 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -623,7 +623,7 @@ extern "C" { void systemBeep(bool) {} void saveConfigAndNotify(void) {} void blackboxFinish(void) {} - bool isAccelerationCalibrationComplete(void) { return true; } + bool accIsCalibrationComplete(void) { return true; } bool isBaroCalibrationComplete(void) { return true; } bool isGyroCalibrationComplete(void) { return gyroCalibDone; } void gyroStartCalibration(bool) {}