MAVLINKHUD

Harmonic Notch Filtering

Executive Summary

Propeller noise is the enemy of good tuning. It injects high-frequency vibration into the gyro signal, which the D-term amplifies, causing hot motors.

The Dynamic Harmonic Notch is a tracking filter that moves its center frequency in real-time to match the motor RPM, effectively surgically removing the noise without adding excessive phase lag at lower frequencies.

Theory & Concepts

1. The Motor Noise Peak

Spinning propellers generate vibration at the fundamental frequency ($f = \text{RPM} / 60$) and its harmonics ($2f, 3f$).

  • Tracking: As throttle increases, RPM increases, and the noise frequency shifts up. The filter must track this.
  • Bandwidth vs. Lag: A narrow notch adds little lag but might miss the peak if tracking is slow. A wide notch catches the peak but adds more lag.

2. Sources of Truth

  • Throttle: Estimates RPM based on MOT_THST_HOVER and a linearization curve. Fast, but less accurate.
  • ESC Telemetry: Real RPM from the ESCs. Accurate, but has latency/transport delay.
  • FFT: Analyzes the gyro data itself to find the peak. Accurate but computationally expensive.

Codebase Investigation

1. The Update Logic: HarmonicNotchFilter::update()

Located in libraries/Filter/HarmonicNotchFilter.cpp.

  • Inputs: center_freq_hz (calculated from RPM/Throttle).
  • Process:
    1. Constrains frequency within Nyquist limits.
    2. Calculates A (Attenuation) and Q (Quality Factor) for the biquad.
    3. Updates the coefficients of the underlying NotchFilter<T>.

2. Harmonics

The class manages multiple filters (_filters[]) to target $1f, 2f, 3f$, etc.

const float notch_center = constrain_float(center_freq_hz[center_n], 0.0f, nyquist_limit);
set_center_frequency(_num_enabled_filters++, notch_center, 1.0, harmonic_mul);

Source Code Reference

Practical Guide: Configuring the Notch

1. Throttle-Based (Mode 1)

  • Hover: Hover the drone. Note the Throttle % (e.g., 0.35) and the dominant noise frequency from the FFT log (e.g., 180Hz).
  • Calculate:
  • Result: At 70% throttle, the filter will track to $\approx 180 * \sqrt{0.70/0.35} = 254\text{Hz}$.

2. ESC Telemetry (Mode 4)

  • Setup: Enable BLHeli_32/S telemetry.
  • Set: INS_HNTCH_MODE = 4.
  • Check: INS_HNTCH_REF = 1 (Scaling factor).
  • Benefit: Exact RPM matching, handles distinct RPMs (e.g., in a turn).