Merge pull request #3191 from iNavFlight/dzikuvx-stage2-fir2-gyro-filter

Port of FIR2 second stage gyro filter from Betaflight
iNavFlight · May 12, 2018 · fd790f1 · fd790f1
2 parents d934460 + 7839680
commit fd790f1
Show file tree

Hide file tree

Showing 8 changed files with 71 additions and 3 deletions.
diff --git a/docs/Cli.md b/docs/Cli.md
@@ -379,6 +379,7 @@ Re-apply any new defaults as desired.
 | acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 | dterm_lpf_hz | 40 | |
 | yaw_lpf_hz | 30 | |
+| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental |
 | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
 | yaw_p_limit | 300 | |
 | iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) |

diff --git a/src/main/build/debug.h b/src/main/build/debug.h
@@ -57,6 +57,7 @@ typedef enum {
  DEBUG_SBUS,
  DEBUG_FPORT,
  DEBUG_ALWAYS,
+ DEBUG_STAGE2,
  DEBUG_COUNT
 } debugType_e;
 

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
@@ -116,6 +116,30 @@ void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t s
  biquadFilterInit(filter, filterFreq, samplingIntervalUs, BIQUAD_Q, FILTER_LPF);
 }
 
+// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k
+void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, uint32_t samplingIntervalUs)
+{
+ if (f_cut < (1000000 / samplingIntervalUs / 2)) {
+ const float dT = (float) samplingIntervalUs * 0.000001f;
+ const float RC = 1.0f / ( 2.0f * M_PIf * f_cut );
+ const float k = dT / (RC + dT);
+ filter->b0 = k / 2;
+ filter->b1 = k / 2;
+ filter->b2 = 0;
+ filter->a1 = -(1 - k);
+ filter->a2 = 0;
+ } else {
+ filter->b0 = 1.0f;
+ filter->b1 = 0.0f;
+ filter->b2 = 0.0f;
+ filter->a1 = 0.0f;
+ filter->a2 = 0.0f;
+ }
+
+ // zero initial samples
+ filter->d1 = filter->d2 = 0;
+}
+
 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType)
 {
  // Check for Nyquist frequency and if it's not possible to initialize filter as requested - set to no filtering at all

diff --git a/src/main/common/filter.h b/src/main/common/filter.h
@@ -65,6 +65,8 @@ void pt1FilterReset(pt1Filter_t *filter, float input);
 void rateLimitFilterInit(rateLimitFilter_t *filter);
 float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);
 
+void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, uint32_t samplingIntervalUs);
+
 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz);
 void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs);
 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType);

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -67,7 +67,7 @@ tables:
  - name: i2c_speed
  values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
  - name: debug_modes
- values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS"]
+ values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2"]
  - name: async_mode
  values: ["NONE", "GYRO", "ALL"]
  - name: aux_operator
@@ -133,6 +133,11 @@ groups:
  condition: USE_GYRO_NOTCH_2
  min: 1
  max: 500 
+ - name: gyro_stage2_lowpass_hz
+ field: gyro_stage2_lowpass_hz
+ condition: USE_GYRO_BIQUAD_RC_FIR2
+ min: 0
+ max: 500
  - name: gyro_to_use
  condition: USE_DUAL_GYRO
  min: 0

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -93,7 +93,13 @@ STATIC_FASTRAM filterApplyFnPtr notchFilter2ApplyFn;
 STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2);
+#if defined(USE_GYRO_BIQUAD_RC_FIR2)
+// gyro biquad RC FIR2 filter
+STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn;
+STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT];
+#endif
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
  .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros
@@ -106,7 +112,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
  .gyro_soft_notch_hz_1 = 0,
  .gyro_soft_notch_cutoff_1 = 1,
  .gyro_soft_notch_hz_2 = 0,
- .gyro_soft_notch_cutoff_2 = 1
+ .gyro_soft_notch_cutoff_2 = 1,
+ .gyro_stage2_lowpass_hz = 0
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -269,6 +276,19 @@ void gyroInitFilters(void)
  notchFilter2ApplyFn = nullFilterApply;
 #endif
 
+#ifdef USE_GYRO_BIQUAD_RC_FIR2
+ STATIC_FASTRAM biquadFilter_t gyroFilterStage2[XYZ_AXIS_COUNT];
+ gyroFilterStage2ApplyFn = nullFilterApply;
+
+ if (gyroConfig()->gyro_stage2_lowpass_hz > 0) {
+ gyroFilterStage2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+ for (int axis = 0; axis < 3; axis++) {
+ stage2Filter[axis] = &gyroFilterStage2[axis];
+ biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getGyroUpdateRate());
+ }
+ }
+#endif
+
  if (gyroConfig()->gyro_soft_lpf_hz) {
  softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
  for (int axis = 0; axis < 3; axis++) {
@@ -421,6 +441,18 @@ void gyroUpdate(timeDelta_t gyroUpdateDeltaUs)
 
  DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
+ if (axis < 2) {
+ DEBUG_SET(DEBUG_STAGE2, axis, lrintf(gyroADCf));
+ }
+
+#ifdef USE_GYRO_BIQUAD_RC_FIR2
+ gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf);
+#endif
+
+ if (axis < 2) {
+ DEBUG_SET(DEBUG_STAGE2, axis + 2, lrintf(gyroADCf));
+ }
+
  gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
 
  DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));

diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
@@ -57,6 +57,7 @@ typedef struct gyroConfig_s {
  uint16_t gyro_soft_notch_cutoff_1;
  uint16_t gyro_soft_notch_hz_2;
  uint16_t gyro_soft_notch_cutoff_2;
+ uint16_t gyro_stage2_lowpass_hz;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

diff --git a/src/main/target/common.h b/src/main/target/common.h
@@ -45,6 +45,8 @@
 #define USE_TELEMETRY_LTM
 #define USE_TELEMETRY_FRSKY
 
+#define USE_GYRO_BIQUAD_RC_FIR2
+
 #if defined(STM_FAST_TARGET)
 #define SCHEDULER_DELAY_LIMIT 10
 #else