mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 17:55:30 +03:00
Add GYRO_CALIBRATION debug mode (#12241)
This commit is contained in:
parent
7dedaa4254
commit
85d9ae2386
3 changed files with 22 additions and 11 deletions
|
@ -108,4 +108,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
||||||
"VTX_MSP",
|
"VTX_MSP",
|
||||||
"GPS_DOP",
|
"GPS_DOP",
|
||||||
"FAILSAFE",
|
"FAILSAFE",
|
||||||
|
"GYRO_CALIBRATION",
|
||||||
};
|
};
|
||||||
|
|
|
@ -106,6 +106,7 @@ typedef enum {
|
||||||
DEBUG_VTX_MSP,
|
DEBUG_VTX_MSP,
|
||||||
DEBUG_GPS_DOP,
|
DEBUG_GPS_DOP,
|
||||||
DEBUG_FAILSAFE,
|
DEBUG_FAILSAFE,
|
||||||
|
DEBUG_GYRO_CALIBRATION,
|
||||||
DEBUG_COUNT
|
DEBUG_COUNT
|
||||||
} debugType_e;
|
} debugType_e;
|
||||||
|
|
||||||
|
|
|
@ -91,7 +91,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev;
|
||||||
#endif
|
#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_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)
|
#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)
|
STATIC_UNIT_TESTED NOINLINE void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
|
||||||
{
|
{
|
||||||
|
bool calFailed = false;
|
||||||
|
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
// Reset g[axis] at start of calibration
|
// Reset g[axis] at start of calibration
|
||||||
if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
|
if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
|
||||||
|
@ -220,26 +222,32 @@ STATIC_UNIT_TESTED NOINLINE void performGyroCalibration(gyroSensor_t *gyroSensor
|
||||||
|
|
||||||
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
|
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
|
||||||
const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
|
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
|
// into the spare field - debug[3], in DEBUG_GYRO_RAW
|
||||||
if (axis == X) {
|
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
|
// check deviation and startover in case the model was moved
|
||||||
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
|
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
|
||||||
gyroSetCalibrationCycles(gyroSensor);
|
calFailed = true;
|
||||||
return;
|
} else {
|
||||||
}
|
// please take care with exotic boardalignment !!
|
||||||
|
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
|
||||||
// please take care with exotic boardalignment !!
|
if (axis == Z) {
|
||||||
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
|
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
|
||||||
if (axis == Z) {
|
}
|
||||||
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (calFailed) {
|
||||||
|
gyroSetCalibrationCycles(gyroSensor);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
|
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
|
||||||
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
|
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
|
||||||
if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
|
if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
|
||||||
|
@ -248,6 +256,7 @@ STATIC_UNIT_TESTED NOINLINE void performGyroCalibration(gyroSensor_t *gyroSensor
|
||||||
}
|
}
|
||||||
|
|
||||||
--gyroSensor->calibration.cyclesRemaining;
|
--gyroSensor->calibration.cyclesRemaining;
|
||||||
|
DEBUG_SET(DEBUG_GYRO_CALIBRATION, 3, gyroSensor->calibration.cyclesRemaining);
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(USE_GYRO_SLEW_LIMITER)
|
#if defined(USE_GYRO_SLEW_LIMITER)
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue