Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 09:45:37 +03:00
Code Issues Wiki Activity

Add GYRO_CALIBRATION debug mode (#12241)

Browse source
This commit is contained in:
Steve Evans 2023-01-30 09:05:48 +00:00 • committed by GitHub
parent 7dedaa4254
commit 85d9ae2386
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 22 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.c
View file

@ -108,4 +108,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"VTX_MSP",
"GPS_DOP",
"FAILSAFE",
"GYRO_CALIBRATION",
};

1
src/main/build/debug.h
View file

@ -106,6 +106,7 @@ typedef enum {
DEBUG_VTX_MSP,
DEBUG_GPS_DOP,
DEBUG_FAILSAFE,
DEBUG_GYRO_CALIBRATION,
DEBUG_COUNT
} debugType_e;

31
src/main/sensors/gyro.c
View file

@ -91,7 +91,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev;
#endif
#define DEBUG_GYRO_CALIBRATION 3
#define DEBUG_GYRO_CALIBRATION_VALUE 3
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
@ -205,6 +205,8 @@ bool isFirstArmingGyroCalibrationRunning(void)
STATIC_UNIT_TESTED NOINLINE void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
{
bool calFailed = false;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
@ -220,26 +222,32 @@ STATIC_UNIT_TESTED NOINLINE void performGyroCalibration(gyroSensor_t *gyroSensor
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
// DEBUG_GYRO_CALIBRATION records the standard deviation of roll
// DEBUG_GYRO_CALIBRATION_VALUE records the standard deviation of roll
// into the spare field - debug[3], in DEBUG_GYRO_RAW
if (axis == X) {
DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION_VALUE, lrintf(stddev));
}
DEBUG_SET(DEBUG_GYRO_CALIBRATION, axis, stddev);
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(gyroSensor);
return;
}
// please take care with exotic boardalignment !!
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
if (axis == Z) {
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
calFailed = true;
} else {
// please take care with exotic boardalignment !!
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
if (axis == Z) {
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
}
}
}
}
if (calFailed) {
gyroSetCalibrationCycles(gyroSensor);
return;
}
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
@ -248,6 +256,7 @@ STATIC_UNIT_TESTED NOINLINE void performGyroCalibration(gyroSensor_t *gyroSensor
}
--gyroSensor->calibration.cyclesRemaining;
DEBUG_SET(DEBUG_GYRO_CALIBRATION, 3, gyroSensor->calibration.cyclesRemaining);
}
#if defined(USE_GYRO_SLEW_LIMITER)