RPM Sensors: The Harmonic Notch

Executive Summary

Vibration is the enemy of stability. Propellers create massive vibration spikes at specific frequencies related to their RPM. A static Low-Pass Filter is blunt—it cuts noise but adds latency (lag), making the drone feel sluggish. The Dynamic Harmonic Notch Filter is a surgical tool: it tracks the RPM of the motors in real-time and deletes only the propeller noise, leaving the pilot's control inputs untouched.

Theory & Concepts

1. Vibration Physics (The Propeller)

A spinning propeller is an unbalanced mass.

Fundamental Frequency (1x): RPM / 60. If props spin at 6000 RPM, the noise is at 100 Hz.
Harmonics (2x, 3x): Blades flex, motors cog. This creates echoes at 200 Hz, 300 Hz, etc.
The Target: We want to erase these specific spikes from the gyro data.

2. Signal Aliasing (The "Wagon Wheel" Effect)

If your IMU samples at 1kHz, but the vibration is at 900Hz, the vibration will "fold back" (alias) and look like 100Hz noise.

Why Notch Filters Matter: By removing the noise before it aliases (using high-rate sampling and filtering), we prevent the EKF from seeing ghost motion.

3. Notch vs. Low Pass

Low Pass: Like a heavy blanket. Muffles everything above 50Hz.
- Pro: Simple.
- Con: Delays the signal. The drone reacts late to gusts.
Notch: Like a laser scalpel. Removes 100Hz +/- 5Hz.
- Pro: Zero latency for other frequencies.
- Con: Needs to know exactly where the noise is. This is why RPM sensors are critical.

Architecture (The Engineer's View)

1. The Source (`AP_RPM`)

ESC Telemetry: Bi-directional DShot reports the RPM of every motor hundreds of times per second.
Hall Sensor: A magnet on the bell triggers a GPIO pin interrupt.
Normalization: AP_RPM converts these disparate sources into a unified RPM float value.

2. The Frequency Calculation (`AP_Vehicle`)

The vehicle logic converts RPM to Frequency (Hz).

Fundamental Frequency: Freq = RPM / 60.
Scaling: INS_HNTCH_REF scales this value (e.g., if you want to target the 2nd harmonic).
Update Rate: The filter center frequency is updated at 200Hz (or Loop Rate if configured).

3. The Filter (`HarmonicNotchFilter`)

This is a digital signal processing filter running on the Gyro/Accel data.

Inputs: Raw Gyro Data (1kHz - 8kHz).
Mechanism: It creates a "Notch" (infinite attenuation) at the target frequency.
Result: The noise is removed before it hits the PID controllers. This allows you to raise P-Gains significantly higher without oscillation.

Key Parameters

Parameter	Default	Description
`INS_HNTCH_ENABLE`	0	Master switch.
`INS_HNTCH_MODE`	0	1=Throttle, 4=RPM (Best).
`INS_HNTCH_FREQ`	80	(Hz) Base frequency (fallback if RPM lost).
`INS_HNTCH_BW`	40	(Hz) Bandwidth (width of the notch).

Source Code Reference

Update Logic: AP_Vehicle::update_dynamic_notch()

Practical Guide: Setting Up Bi-Directional DShot Notch

This is the standard setup for modern quads using BLHeli_32 or AM32 ESCs.

Prerequisites

ESC Protocol: MOT_PWM_TYPE = 4 (DShot150), 5 (DShot300), or 6 (DShot600).
Bi-Directional: SERVO_BLH_BDMASK must cover your motor channels (e.g., 15 for first 4 motors).
Verification: Check the "Status" tab in Mission Planner. You should see esc1_rpm, esc2_rpm, etc., changing when you spin the motors.

Configuration

Enable the Notch:
- INS_HNTCH_ENABLE = 1
Set the Mode:
- INS_HNTCH_MODE = 3 (ESC Telemetry). This tells ArduPilot to read the RPM from the DShot stream.
Configure Frequency Mapping:
- INS_HNTCH_REF = 1.0. (Use the raw RPM / 60 as Hz).
- INS_HNTCH_FREQ = 80 (Hz). This is a safe fallback if RPM data is lost.
- INS_HNTCH_BW = 40 (Hz).
Target Harmonics:
- INS_HNTCH_HMNCS = 3 (Target 1st and 2nd harmonics). This is usually sufficient.

Verification (The FFT Graph)

Set INS_LOG_BAT_MASK = 1 (Log Pre-Filter Gyro).
Fly a short hover.
Download the log and perform an FFT Analysis in Mission Planner (Ctrl+F -> FFT).
Before: You should see a giant spike at ~100-200Hz (the prop noise).
After: That spike should be completely gone in the "Post-Filter" graph.

For more details, see the ArduPilot Wiki: Harmonic Notch Filter.