Navigation Architecture

Executive Summary

Navigation in ArduPilot is not a monolithic block; it is a layered architecture separating Mission Management (high-level intent) from Position Control (kinematic execution) and Attitude Control (stabilization).

This separation allows AP_Mission to remain vehicle-agnostic, simply feeding "Target Locations" to vehicle-specific libraries (AC_WPNav for Copter, AP_L1_Control for Plane).

Theory & Concepts

The Control Loop Hierarchy

The autopilot operates on a cascaded control loop architecture, where slower outer loops drive faster inner loops.

1. Mission Layer (10Hz): AP_Mission determines the active command (e.g., "Fly to Waypoint 4"). It monitors completion criteria (distance, altitude, time) and advances the index.
2. Navigation Layer (50Hz):
   • Copter: AC_WPNav generates a kinematic path (Spline or Straight Line) to the target. It outputs a Desired Velocity Vector and Position Target.
   • Plane: AP_L1_Control calculates a Lateral Acceleration to minimize cross-track error (L1 Guidance).
3. Position/Velocity Layer (50Hz - 400Hz): Converts Desired Velocity/Acceleration into Desired Lean Angles (Copter) or Control Surface Deflections (Plane).
4. Attitude Layer (400Hz+): The Rate Controller drives the motors/servos to achieve the Desired Lean Angles.

Reference Frames

• NEU (North-East-Up): The standard internal frame for navigation relative to the EKF Origin (Home).
• Body Frame: X-forward, Y-right, Z-down. Used for sensor data and low-level control.
• Terrain Frame: Altitude relative to the loaded terrain database or rangefinder.

Codebase Investigation

1. The Mission Director: AP_Mission::update()

Located in libraries/AP_Mission/AP_Mission.cpp, this function is the heartbeat of autonomous operations.

• Logic:
  1. Checks verify_command(_nav_cmd) to see if the current objective is met.
  2. If true, it calls advance_current_nav_cmd() to load the next instruction.
  3. It also manages "Do Commands" (servos, relays) which run concurrently with navigation.

2. Vehicle Implementation: Copter vs. Plane

Copter (ArduCopter/mode_auto.cpp)

When ModeAuto::run() executes:

• It calls wp_nav->update_wpnav().
• This moves the "Leash" (Target Position) towards the destination.
• The Position Controller (pos_control->update_z_controller()) pulls the drone towards the Leash.

Plane (ArduPlane/mode_auto.cpp)

Plane uses a different strategy based on fluid dynamics:

• It calculates nav_roll_cd (Commanded Centi-Degrees Roll) using L1 logic to track the line between waypoints.
• It calculates nav_pitch_cd and throttle using TECS (Total Energy Control System) to manage Speed and Height simultaneously.

Source Code Reference

• Mission Logic: libraries/AP_Mission/AP_Mission.cpp
• Copter Auto: ArduCopter/mode_auto.cpp
• Plane Auto: ArduPlane/mode_auto.cpp

Practical Guide: Monitoring Mission Status

Operators often need to know why a drone is pausing or what it is waiting for.

1. MAVLink Messages

Monitor MISSION_CURRENT (Msg ID 42).

• seq: The index of the active command.
• If seq stops incrementing, the drone is executing that command.

2. Common "Stuck" Scenarios

• Takeoff Wait: If the drone arms but doesn't spin up in Auto, check MIS_OPTIONS. Some configurations wait for a "Mission Start" MAVLink command.
• Altitude Wait: If verify_nav_wp never completes, check your Altitude Acceptance Radius (WPNAV_RADIUS or WP_RADIUS). If the drone cannot reach the exact Z-altitude due to baro drift or max throttle, it will circle indefinitely.

3. Debugging with Logs

Open the DataFlash log and look for CMD messages.

• CTot: Command Total.
• CNum: Current Command Number.
• CId: Command ID (e.g., 16 = Waypoint, 22 = Takeoff).
• Tip: If CNum changes but the drone behaves oddly, check the P1 (Parameter 1) field. For Loiter commands, this is often the "Time" or "Turns" which might be set incorrectly to 0 (forever).