MAVLINKHUD

Rangefinders: Tilt Compensation

Executive Summary

Rangefinders (Lidar, Sonar, Radar) provide the critical "Height Above Ground" (AGL) metric needed for automated landing and terrain following. While a Barometer tells you how high you are above your takeoff point, a Rangefinder tells you how close you are to crashing into the hill you are flying over. ArduPilot applies sophisticated trigonometry to convert the raw "slant range" from these sensors into a useful vertical altitude.

Theory & Concepts

1. Sensor Physics: Light vs. Sound vs. Radio

Not all rangefinders are created equal.

  • Lidar (Light Detection and Ranging): Uses pulsed laser light.
    • Pros: Extremely fast update rates (500Hz+), very precise (mm).
    • Cons: Fails on Black Surfaces (absorbs light), Mirrors/Water (reflects light away), and in Bright Sunlight (saturation). It is also blinded by fog/dust.
  • Sonar (Ultrasonic): Uses sound waves.
    • Pros: Works on glass/water/transparent surfaces. Immune to light conditions.
    • Cons: Very slow (speed of sound is slow). Wide "cone" picks up grass/obstacles easily. Noisy in prop-wash (air turbulence).
  • Radar (Radio Detection): Uses mmWave radio.
    • Pros: Sees through dust, fog, and light rain. Robust against surface type.
    • Cons: Larger, heavier, more expensive.

2. The "Slant Range" Problem

Rangefinders are "dumb." They measure the distance to the first thing they hit. If you pitch the drone forward 45 degrees, the sensor points forward, not down.

  • The Error: The measured distance becomes the Hypotenuse of the triangle.
  • The Result: If you don't correct for this, pitching forward makes the drone think it is "climbing" (distance increasing), so the autopilot cuts the throttle to compensate, causing a crash.

3. Surface Texture & divergence

  • Beam Divergence: A laser beam spreads out over distance. At 50m, the "dot" might be 1m wide.
  • The "Edge" Problem: If the beam hits the edge of a building, half the light returns from the roof (5m away) and half from the ground (20m away). The sensor averages this to 12.5m—a ghost measurement that exists nowhere.
  • Filtering: ArduPilot uses a "Glitch Filter" to reject sudden jumps that are physically impossible for the drone to fly.

Architecture (The Engineer's View)

1. The Raw Measurement

The AP_RangeFinder library reports the raw distance returned by the hardware driver.

  • Input: Distance_Raw.
  • Correction: None (at this layer).

2. The Trigonometry Correction

The correction logic resides in AP_SurfaceDistance::update() (or inside the EKF).

  • Formula: Altitude = Distance_Raw * cos(Roll) * cos(Pitch)
  • Mechanism: It accesses the Attitude Heading Reference System (AHRS) to get the current rotation matrix. It projects the laser vector onto the vertical Z-axis.

3. The "Cone Effect" (Geometric Error)

Even with perfect math, physics gets in the way.

  • Scenario: You bank 30 degrees right over a slope.
  • The Problem: The laser is now hitting the ground upslope or downslope relative to the drone. The math assumes the ground is flat. This results in "Phantom Altitude Jumps" when maneuvering over rough terrain.

Key Parameters

Parameter Default Description
RNGFND1_TYPE 0 Driver selection (e.g., LightWare, MaxBotix, Benewake).
RNGFND1_ORIENT 25 (Down). Defines which way the sensor points. If set to 0 (Forward), the math changes completely (Obstacle Avoidance).
RNGFND_GAIN 0.5 Used by AltHold to blend Lidar vs Baro. Higher gain trusts Lidar more.

Source Code Reference