MAVLINKHUD

Frequency Domain Analysis

Executive Summary

In advanced tuning, we stop thinking of the drone as a time-domain machine (reacting to inputs) and start thinking of it in the Frequency Domain (reacting to vibrations and oscillations).

Understanding the sampling rate, the Nyquist limit, and filter cutoffs is essential for dealing with propeller noise and avoiding "D-term heating."

Theory & Concepts

1. The Nyquist Limit

If your gyro samples at 1kHz, the maximum frequency it can accurately measure is 500Hz (Nyquist Frequency).

  • Aliasing: Vibrations above 500Hz don't disappear; they fold back into the lower spectrum (e.g., a 600Hz vibe looks like 400Hz).
  • Solution: We must filter out high-frequency noise before it down-samples, or use a very high sampling rate (Fast Sampling).

2. Digital Filters (Biquads)

ArduPilot uses 2nd Order Biquad Low-Pass Filters (LowPassFilter2p).

  • Phase Lag: Every filter introduces delay. A 20Hz cutoff filter has much more lag than a 100Hz filter.
  • The Tradeoff: Lower cutoff = smoother signal but slower reaction (worse PID performance).

Codebase Investigation

1. The Loop Rate: AP_InertialSensor::init()

Located in libraries/AP_InertialSensor/AP_InertialSensor.cpp.

  • _loop_rate sets the main loop speed (e.g., 400Hz).
  • _sample_period_usec is derived from this.
  • Fast Sampling: If _fast_sampling_mask is enabled, the backend reads the gyro FIFO at 1kHz-8kHz, applies a hardware low-pass, and then averages down to the loop rate.

2. The Filter Implementation: LowPassFilter2p

Located in libraries/Filter/LowPassFilter2p.cpp.

  • It implements a standard Direct Form I or II transposed biquad.
  • Math: 
    T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
T output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
    This mathematical recurrence relation determines how past inputs affect the current output (Infinite Impulse Response).

Source Code Reference

Practical Guide: Setting Filter Frequency

1. INS_GYRO_FILTER

  • Standard: 20Hz for large quads, 40-80Hz for small racers.
  • Tuning: Raise this value to reduce phase lag (improves flight feel). If motors get hot, lower it.

2. INS_ACCEL_FILTER

  • Standard: 10-20Hz.
  • Role: The accelerometer is NOT used in the fast rate loop (PID), only for angle estimation (Level horizon). High filtering here is fine and desirable to prevent horizon drift.