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

gyro sensors/debug: standardize gyro debugging enum elements

Browse source 
* will require coordination with BFC of course.
* rationale: previously `DEBUG_GYRO_NOTCH` debugging was used to grab the
  scaled, unfiltered gyro readings, prior to the FFT running. This has been
  updated to `DEBUG_GYRO_SCALED`.

  similarly, `DEBUG_GYRO` debugging was used to record the filtered gyro. This
  is updated to `GYRO_FILTERED`.

  interestingly, `DEBUG_GYRO` was also used for movement threshold calibration.
  This has been updated to be `DEBUG_GYRO_CALIBRATION` and also now stores
  per-axis standard deviation.

  Application of filter position moved slightly for logical grouping, has no
  effect due to LTI.
This commit is contained in:
AJ Christensen 2018-06-07 17:17:26 +12:00
parent de22f87a6d
commit f2cc9acc1f
2 changed files with 13 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -52,10 +52,10 @@ typedef enum {
DEBUG_NONE, DEBUG_NONE,
DEBUG_CYCLETIME, DEBUG_CYCLETIME,
DEBUG_BATTERY, DEBUG_BATTERY,
DEBUG_GYRO, DEBUG_GYRO_FILTERED,
DEBUG_ACCELEROMETER, DEBUG_ACCELEROMETER,
DEBUG_PIDLOOP, DEBUG_PIDLOOP,
DEBUG_GYRO_NOTCH, DEBUG_GYRO_SCALED,
DEBUG_RC_INTERPOLATION, DEBUG_RC_INTERPOLATION,
DEBUG_ANGLERATE, DEBUG_ANGLERATE,
DEBUG_ESC_SENSOR, DEBUG_ESC_SENSOR,

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

@ -520,9 +520,9 @@ bool gyroInit(void)
switch (debugMode) { switch (debugMode) {
case DEBUG_FFT: case DEBUG_FFT:
case DEBUG_GYRO_NOTCH:
case DEBUG_GYRO:
case DEBUG_GYRO_RAW: case DEBUG_GYRO_RAW:
case DEBUG_GYRO_SCALED:
case DEBUG_GYRO_FILTERED:
gyroDebugMode = debugMode; gyroDebugMode = debugMode;
break; break;
default: default:
@ -864,8 +864,8 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
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 per-axis standard deviation
DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); DEBUG_SET(DEBUG_GYRO_CALIBRATION, axis, lrintf(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) {
@ -1054,14 +1054,13 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
// NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
#endif #endif
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) { if (!gyroSensor->overflowDetected) {
@ -1075,11 +1074,8 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]);
// scale gyro output to degrees per second // scale gyro output to degrees per second
float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
// DEBUG_GYRO_NOTCH records the unfiltered gyro output // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output
DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf)); DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf));
// apply lowpass2 filter
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
// apply dynamic notch filter // apply dynamic notch filter
@ -1093,14 +1089,13 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
} }
} }
#endif #endif
// apply static notch filters and software lowpass filters
// apply static notch filters
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
// apply lowpass2 filter
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
// DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
if (!gyroSensor->overflowDetected) { if (!gyroSensor->overflowDetected) {