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:
parent
ff0628193f
commit
d90b42970c
10 changed files with 39 additions and 40 deletions
|
@ -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
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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));
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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 :
|
||||||
|
|
|
@ -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) {}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue