1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00

Acc fn/variable renames to align with iNav

This commit is contained in:
Martin Budden 2017-12-29 09:56:04 +00:00
parent ff0628193f
commit d90b42970c
10 changed files with 39 additions and 40 deletions

View file

@ -289,7 +289,7 @@ typedef struct blackboxMainState_s {
int16_t rcCommand[4]; int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT]; 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 debug[DEBUG16_VALUE_COUNT];
int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS]; int16_t servo[MAX_SUPPORTED_SERVOS];
@ -580,7 +580,7 @@ static void writeIntraframe(void)
blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { 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)) { 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: //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT); blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) { 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)) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT); 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_I[i] = axisPID_I[i];
blackboxCurrent->axisPID_D[i] = axisPID_D[i]; blackboxCurrent->axisPID_D[i] = axisPID_D[i];
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
blackboxCurrent->accSmooth[i] = acc.accSmooth[i]; blackboxCurrent->accADC[i] = acc.accADC[i];
#ifdef USE_MAG #ifdef USE_MAG
blackboxCurrent->magADC[i] = mag.magADC[i]; blackboxCurrent->magADC[i] = mag.magADC[i];
#endif #endif

View file

@ -134,7 +134,7 @@ static bool isCalibrating(void)
// Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG // 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) void resetArmingDisabled(void)

View file

@ -200,9 +200,9 @@ static void imuCalculateAcceleration(uint32_t deltaT)
const float dT = (float)deltaT * 1e-6f; const float dT = (float)deltaT * 1e-6f;
t_fp_vector accel_ned; t_fp_vector accel_ned;
accel_ned.V.X = acc.accSmooth[X]; accel_ned.V.X = acc.accADC[X];
accel_ned.V.Y = acc.accSmooth[Y]; accel_ned.V.Y = acc.accADC[Y];
accel_ned.V.Z = acc.accSmooth[Z]; accel_ned.V.Z = acc.accADC[Z];
imuTransformVectorBodyToEarth(&accel_ned); imuTransformVectorBodyToEarth(&accel_ned);
@ -392,7 +392,7 @@ static bool imuIsAccelerometerHealthy(void)
{ {
float accMagnitude = 0; float accMagnitude = 0;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
const float a = acc.accSmooth[axis]; const float a = acc.accADC[axis];
accMagnitude += a * a; accMagnitude += a * a;
} }
@ -478,9 +478,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
imuCalculateEstimatedAttitude(currentTimeUs); imuCalculateEstimatedAttitude(currentTimeUs);
IMU_UNLOCK; IMU_UNLOCK;
} else { } else {
acc.accSmooth[X] = 0; acc.accADC[X] = 0;
acc.accSmooth[Y] = 0; acc.accADC[Y] = 0;
acc.accSmooth[Z] = 0; acc.accADC[Z] = 0;
} }
} }

View file

@ -779,7 +779,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
} }
for (int i = 0; i < 3; i++) { 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++) { for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroRateDps(i)); sbufWriteU16(dst, gyroRateDps(i));

View file

@ -476,7 +476,7 @@ static void showSensorsPage(void)
i2c_OLED_send_string(bus, " X Y Z"); i2c_OLED_send_string(bus, " X Y Z");
if (sensors(SENSOR_ACC)) { 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(); padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++); i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer); i2c_OLED_send_string(bus, lineBuffer);

View file

@ -231,11 +231,10 @@ retry:
case ACC_ICM20608G: case ACC_ICM20608G:
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500) #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
#ifdef USE_ACC_SPI_MPU6500 #ifdef USE_ACC_SPI_MPU6500
if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
#else #else
if (mpu6500AccDetect(dev)) if (mpu6500AccDetect(dev)) {
#endif #endif
{
#ifdef ACC_MPU6500_ALIGN #ifdef ACC_MPU6500_ALIGN
dev->accAlign = ACC_MPU6500_ALIGN; dev->accAlign = ACC_MPU6500_ALIGN;
#endif #endif
@ -371,7 +370,7 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
calibratingA = calibrationCyclesRequired; calibratingA = calibrationCyclesRequired;
} }
bool isAccelerationCalibrationComplete(void) bool accIsCalibrationComplete(void)
{ {
return calibratingA == 0; return calibratingA == 0;
} }
@ -393,14 +392,15 @@ static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration // Reset a[axis] at start of calibration
if (isOnFirstAccelerationCalibrationCycle()) if (isOnFirstAccelerationCalibrationCycle()) {
a[axis] = 0; a[axis] = 0;
}
// Sum up CALIBRATING_ACC_CYCLES readings // 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 // Reset global variables to prevent other code from using un-calibrated data
acc.accSmooth[axis] = 0; acc.accADC[axis] = 0;
accelerationTrims->raw[axis] = 0; accelerationTrims->raw[axis] = 0;
} }
@ -420,7 +420,6 @@ static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims
static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{ {
uint8_t axis;
static int32_t b[3]; static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 }; static int16_t accZero_saved[3] = { 0, 0, 0 };
static rollAndPitchTrims_t angleTrim_saved = { { 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; angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
} }
if (InflightcalibratingA > 0) { if (InflightcalibratingA > 0) {
for (axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration // Reset a[axis] at start of calibration
if (InflightcalibratingA == 50) if (InflightcalibratingA == 50)
b[axis] = 0; b[axis] = 0;
// Sum up 50 readings // Sum up 50 readings
b[axis] += acc.accSmooth[axis]; b[axis] += acc.accADC[axis];
// Clear global variables for next reading // Clear global variables for next reading
acc.accSmooth[axis] = 0; acc.accADC[axis] = 0;
accelerationTrims->raw[axis] = 0; accelerationTrims->raw[axis] = 0;
} }
// all values are measured // all values are measured
@ -473,9 +472,9 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP
static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims) static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
{ {
acc.accSmooth[X] -= accelerationTrims->raw[X]; acc.accADC[X] -= accelerationTrims->raw[X];
acc.accSmooth[Y] -= accelerationTrims->raw[Y]; acc.accADC[Y] -= accelerationTrims->raw[Y];
acc.accSmooth[Z] -= accelerationTrims->raw[Z]; acc.accADC[Z] -= accelerationTrims->raw[Z];
} }
void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) 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++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
DEBUG_SET(DEBUG_ACCELEROMETER, axis, acc.dev.ADCRaw[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) { if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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); performAcclerationCalibration(rollAndPitchTrims);
} }
@ -512,7 +511,7 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
++accumulatedMeasurementCount; ++accumulatedMeasurementCount;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulatedMeasurements[axis] += acc.accSmooth[axis]; accumulatedMeasurements[axis] += acc.accADC[axis];
} }
} }

View file

@ -46,7 +46,7 @@ typedef enum {
typedef struct acc_s { typedef struct acc_s {
accDev_t dev; accDev_t dev;
uint32_t accSamplingInterval; uint32_t accSamplingInterval;
int32_t accSmooth[XYZ_AXIS_COUNT]; int32_t accADC[XYZ_AXIS_COUNT];
bool isAccelUpdatedAtLeastOnce; bool isAccelUpdatedAtLeastOnce;
} acc_t; } acc_t;
@ -75,7 +75,7 @@ typedef struct accelerometerConfig_s {
PG_DECLARE(accelerometerConfig_t, accelerometerConfig); PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(uint32_t gyroTargetLooptime); bool accInit(uint32_t gyroTargetLooptime);
bool isAccelerationCalibrationComplete(void); bool accIsCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims); void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims);

View file

@ -173,7 +173,7 @@ static void frSkyHubWriteByteInternal(const char data)
static void sendAccel(void) static void sendAccel(void)
{ {
for (unsigned i = 0; i < 3; i++) { 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);
} }
} }

View file

@ -418,15 +418,15 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
*clearToSend = false; *clearToSend = false;
break; break;
case FSSP_DATAID_ACCX : 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; *clearToSend = false;
break; break;
case FSSP_DATAID_ACCY : 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; *clearToSend = false;
break; break;
case FSSP_DATAID_ACCZ : 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; *clearToSend = false;
break; break;
case FSSP_DATAID_T1 : case FSSP_DATAID_T1 :

View file

@ -623,7 +623,7 @@ extern "C" {
void systemBeep(bool) {} void systemBeep(bool) {}
void saveConfigAndNotify(void) {} void saveConfigAndNotify(void) {}
void blackboxFinish(void) {} void blackboxFinish(void) {}
bool isAccelerationCalibrationComplete(void) { return true; } bool accIsCalibrationComplete(void) { return true; }
bool isBaroCalibrationComplete(void) { return true; } bool isBaroCalibrationComplete(void) { return true; }
bool isGyroCalibrationComplete(void) { return gyroCalibDone; } bool isGyroCalibrationComplete(void) { return gyroCalibDone; }
void gyroStartCalibration(bool) {} void gyroStartCalibration(bool) {}