Vision Navigation: architecture

This page is for developers who want to read, audit or extend the extension. Operators don’t need it. The agent half is a compiled Rust binary. The plugin host execs it directly under systemd, passes the per-plugin socket on the command line, and passes the capability token and agent id through the unit environment. The binary reads frames from the agent’s shared vision frame bus; it never opens a camera. The GCS half is an iframe bundle that renders the Vision Nav tab and the Engage Skill.

Module map

extensions/vision-nav/
  manifest.yaml          # id, permissions, component 198, the Vision Nav tab and the Engage Skill
  config-schema.json     # per-drone config schema
  agent/                 # the Rust agent half
    src/
      main.rs            # binary entry point
      lib.rs             # module declarations
      config.rs          # config model and validation (mirrors config-schema.json)
      live_config.rs     # reads the per-drone keys, polls them once a second, exits 75 on a change
      framing.rs         # luma extraction from nv12, yuv420p and rgb24 frames
      flow.rs            # pyramidal Lucas-Kanade tracker
      estimator.rs       # the Estimator trait, EstimatorOutput and EstimatorState
      estimators.rs      # the three modes and the registry
      scale.rs           # the rangefinder-free height ladder
      rangefinder.rs     # the FC relay and the TF-Luna UART reader
      imu.rs             # MAVLink RAW_IMU source and frame/IMU time aligner
      clock_align.rs     # TIMESYNC clock alignment with the flight controller
      mavlink_emit.rs    # message builders and the component router
      pre_arm.rs         # the mode-aware pre-arm gate
      health.rs          # companion state machine and the navigation heartbeat
      pipeline/
        mod.rs           # lifecycle and the per-frame worker
        camchain.rs      # loads timeshift_cam_imu from camchain.yaml
        status.rs        # companion state, health publish and pre-arm inputs
        subscriptions.rs # MAVLink subscriptions the modes consume
        ticks.rs         # TIMESYNC, companion HEARTBEAT, health and stream-rate ticks
    calibration-helper/  # offline camchain.yaml loaders (Python, not shipped in the archive)
  gcs/                   # the GCS half
  locales/               # translation strings

Start-up

Merge the static config with the live per-drone keys and start the config watcher. The watcher polls the store once a second; on a change it exits with status 75 and the unit’s restart policy brings the plugin back on the new config.
Register MAVLink component 198 as kind vio. The registration needs the mavlink.component.vio permission, which the manifest requests.
Subscribe to the flight controller streams the modes need. A refused subscription is recorded in degradedReasons.
Start the ticks: TIMESYNC (1 Hz), the companion HEARTBEAT (1 Hz), the health publish (1 Hz), and MAV_CMD_SET_MESSAGE_INTERVAL requests for RAW_IMU at 100 Hz and DISTANCE_SENSOR at 20 Hz, repeated every 30 s so a flight controller reboot does not drop them.
Run the estimator on every frame pair from the vision frame bus. While the plugin reads frames, the vision tap holds a hero lease on the video encoder, so a drone on the thumbnail profile still encodes at the hero frame rate. The lease has no expiry and ends when the engine detaches.

The estimator contract

Every estimator answers step(...) with one EstimatorOutput:

pub struct EstimatorOutput {
    pub timestamp_us: i64,
    pub output_mode: OutputMode,        // OpticalFlow | None
    pub state: EstimatorState,          // off | init | converging | converged | degraded | failed
    pub flow_rate_x: Option<f32>,       // raw angular rate, rad/s
    pub flow_rate_y: Option<f32>,
    pub integrated_x: Option<f32>,      // angle swept over the interval, rad
    pub integrated_y: Option<f32>,
    pub integrated_xgyro: Option<f32>,  // gyro rotation over the interval, rad
    pub integrated_ygyro: Option<f32>,
    pub integrated_zgyro: Option<f32>,
    pub flow_quality: Option<i32>,
    pub flow_distance_m: Option<f32>,
    pub flow_scale_source: Option<ScaleSourceLabel>, // rangefinder | baro | gps | vision | static
    pub integration_time_us: Option<i64>,
}

off returns an empty None sample and emits nothing. The two optical-flow estimators fill the flow fields.

A frame becomes a MAVLink message

1. The vision host publishes a frame on the shared frame bus. The frame callback runs on
   the IPC reader task and forwards the frame down a channel; it never blocks.
2. framing::frame_to_gray converts it to an 8-bit grayscale image.
3. The worker pairs (previous frame, current frame, dt) and asks imu::TimeAligner for
   the gyro sample at the frame time, shifted by timeshift_cam_imu when a camchain.yaml
   is loaded.
4. The estimator runs flow::OpticalFlowLk. Frames wider than 640 px are tracked at
   half resolution; the image pyramid is built once per frame pair.
5. mavlink_emit::ComponentRouter sends the sample on component 198 with a
   per-component sequence number:
     ardupilot, inav -> OPTICAL_FLOW (#100) with flow_rate_x/y
     px4             -> OPTICAL_FLOW_RAD (#106)
   An unknown distance is sent as -1. A companion-wired rangefinder adds DISTANCE_SENSOR.
6. The health snapshot updates; the 1 Hz tick publishes the navigation block.

The scale ladder

scale::ScaleLadder serves optical_flow_degraded. Each pick() returns the first healthy rung:

GLOBAL_POSITION_INT.relative_alt (<= 2 s old)      -> quality x0.7, label baro
VFR_HUD.alt minus take-off (<= 2 s old)            -> quality x0.6, label baro
GPS_RAW_INT.alt above the first 3D fix             -> quality x0.4, label gps
  (outdoor = true, 3D fix, eph <= 200, <= 50 m)
static 1.5 m                                       -> quality x0.2, label static

Distances are clamped to 0.3 to 50 m. The degraded estimator multiplies the raw quality by the rung’s factor and reports degraded on the static rung whatever the raw quality.

The pre-arm gate

pre_arm::PreArmGate::evaluate() is a pure function: no MAVLink, no filesystem, no async. It returns a report of checks, each ok, pending or blocking with a detail string, and an armable flag that is true only when every check is ok.

off: no checks.
optical_flow: companion_active, flow_quality (at or above the gate), rangefinder (a configured topology and a fresh reading inside the sensor’s min-max range).
optical_flow_degraded: companion_active, flow_quality, scale_source (rangefinder, baro, gps or vision; static blocks).

The gate is advisory. The report rides the heartbeat; nothing in the agent or Mission Control stops the flight controller from arming.

Companion state

The companion state drives the component-198 HEARTBEAT system_status:

State	`system_status`
inactive	`MAV_STATE_STANDBY` (3)
active	`MAV_STATE_ACTIVE` (4)
critical	`MAV_STATE_CRITICAL` (6)
terminating	`MAV_STATE_FLIGHT_TERMINATION` (8)

A converged estimator makes the companion active. A 2 s degraded or failed streak, or 2 s with no frames, makes it critical. Once a second the plugin publishes a navigation block through ctx.telemetry.extend. Losing frames or getting them back publishes the block at once instead of waiting for the next tick. Keys are camelCase end to end; a snake_case key is dropped at the cloud relay.

{
  "opticalFlowSupported": true,
  "rangefinderTopology": "fc",
  "recommendedCameraId": "/dev/video0",
  "flowQuality": 180,
  "flowRateHz": 29.5,
  "flowDistanceM": 1.25,
  "companionState": "active",
  "mode": "optical_flow",
  "availableEstimators": ["off", "optical_flow", "optical_flow_degraded"],
  "estimatorState": "converged",
  "flowScaleSource": "rangefinder",
  "imuSource": "mavlink-raw-imu",
  "imuRateHz": 100,
  "cameraImuSyncOffsetMs": 4.2,
  "cameraIntrinsicsLoaded": true,
  "preArmReport": {
    "mode": "optical_flow",
    "armable": true,
    "checks": [
      {"id": "companion_active", "severity": "ok", "detail": ""},
      {"id": "flow_quality", "severity": "ok", "detail": "Quality 180/255."},
      {"id": "rangefinder", "severity": "ok", "detail": "Topology: fc, 1.25 m"}
    ]
  },
  "emitErrors": 0,
  "rangefinderError": null,
  "degradedReasons": [],
  "fcParams": []
}

The Vision Nav tab reads this block: the companion pill, the mode card, the Sensors card, the Estimator card, the telemetry charts, Flow health, the Pre-arm card and the fallback banner.

Adding an estimator

Implement the Estimator trait in estimators.rs (or a new module).
Add its id to available_estimators() so the heartbeat advertises it.
Add the mode variant to Mode in config.rs, and to the mode enum in config-schema.json.
Add a branch to PreArmGate::evaluate() with the checks the new mode needs.
Add the mode to the GCS mode card.
Write tests for its state machine and output shape.

Use extensions

Extension guides

Module map

Start-up

The estimator contract

A frame becomes a MAVLink message

The scale ladder

The pre-arm gate

Companion state

The navigation heartbeat

Adding an estimator

Next steps

Vision plugins

Overview

​Module map

​Start-up

​The estimator contract

​A frame becomes a MAVLink message

​The scale ladder

​The pre-arm gate

​Companion state

​The navigation heartbeat

​Adding an estimator

​Next steps

Vision plugins

Overview

Module map

Start-up

The estimator contract

A frame becomes a MAVLink message

The scale ladder

The pre-arm gate

Companion state

The navigation heartbeat

Adding an estimator

Next steps