MAVLINKHUD

Mission State Machine

Executive Summary

The ArduPilot Mission State Machine is the "Brain" of Auto mode. It is responsible for sequencing events, ensuring that the drone finishes one task before starting the next.

It distinguishes between three types of commands:

  1. Navigation (NAV): "Go somewhere" (blocking).
  2. Do (DO): "Do something now" (non-blocking).
  3. Condition (COND): "Wait for something" (blocking).

Theory & Concepts

The Command Queue

Conceptually, ArduPilot has two parallel execution queues:

  1. Navigation Queue: Executes NAV_WAYPOINT, NAV_LOITER, NAV_LAND. Only one NAV command is active at a time. The drone physically moves to satisfy this command.
  2. Do Command Queue: Executes DO_SET_SERVO, DO_MOUNT_CONTROL. These run concurrently with the NAV command.
  • Example: A mission might have:
    • Command 1 (NAV): Fly to Waypoint A.
    • Command 2 (DO): Set Camera Tilt to 45 deg.
    • Result: The camera tilts while the drone is flying to Waypoint A.

Condition Commands

Condition commands (e.g., CONDITION_YAW, CONDITION_DELAY) block the navigation queue from advancing but don't stop the vehicle's movement (unless it's a delay). They are "gates" that must be passed.

Codebase Investigation

1. The Verification Loop: verify_command()

Located in libraries/AP_Mission/AP_Mission.cpp (and vehicle-specific implementations).
This function checks if the current command is finished.

  • NAV Commands: Checks distance to target, altitude error, or loiter timer.
  • DO Commands: almost always return true immediately (fire and forget).

2. Advancing the Mission: advance_current_nav_cmd()

When verify_command() returns true:

  1. AP_Mission looks at the next command index.
  2. If it's a DO command, it executes it immediately and increments the index again.
  3. It repeats this until it finds the next NAV command or a COND command.
  4. This effectively "flushes" all instant actions between two waypoints.

3. Jumps and Loops

The MAV_CMD_DO_JUMP command modifies the command index directly.

  • AP_Mission maintains a _jump_tracking array to remember how many times a loop has been repeated.
  • Safety: The array size is limited (AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS), preventing infinite recursion memory leaks.

Source Code Reference

Practical Guide: Debugging Mission Stalls

1. "The Drone Won't Move On"

If the drone reaches a waypoint but just sits there:

  • Check Acceptance Radius: Is WP_RADIUS too small? If GPS drift > Radius, it never "hits" the target.
  • Check Altitude: Is it trying to reach a specific altitude but maxed out on throttle?
  • Check Conditionals: Did you put a CONDITION_YAW that requires it to face North, but the yaw PID is loose?

2. "The Camera Didn't Trigger"

If DO_DIGICAM_CONTROL was skipped:

  • Ensure it wasn't placed after a NAV_LAND or NAV_RTL. Once the mission enters a "Terminal" state, subsequent commands are often ignored.
  • Place DO commands before the NAV command for the leg you want them active on.

3. Infinite Loops

  • Symptom: Drone repeats the same 3 waypoints forever.
  • Fix: Check the MAV_CMD_DO_JUMP "repeat" parameter. -1 or 0 often means infinite.