Architecture

Vision Navigation is built around a single design rule: the camera, the IMU, the rangefinder, and the estimator are independent pieces. Any estimator runs on any combination of camera, IMU, and optional rangefinder; a new estimator plugs in behind a stable contract without rewiring the plugin. The agent half is a compiled Rust binary. The plugin host execs it directly under systemd, hands it the per-plugin socket on the command line, and passes the capability token and agent id through the unit environment. The binary subscribes to the agent’s shared vision frame bus instead of opening a camera itself. Operator-facing behaviour is unchanged from earlier releases; this page documents the shape for developers who want to add a new estimator, extend the calibration loader, or audit the data flow.

Module map (agent half)

The agent crate lives at extensions/vision-nav/agent/. The hot path (frame consumption, optical-flow tracking, the six-mode degradation state machine, the scale ladder, the pre-arm gate, the MAVLink component router, TIMESYNC clock alignment, IMU sourcing, rangefinder drivers, the VIO vendor-binary bridge, and the heartbeat snapshot) is Rust.

extensions/vision-nav/
  agent/                 # the compiled Rust agent half
    Cargo.toml
    src/
      main.rs          # binary entry point: builds the plugin, runs to SIGTERM
      lib.rs           # module declarations
      config.rs        # per-drone config model + validation (mirrors config-schema.json)
      framing.rs       # luma extraction from the shared-bus frame formats
      flow.rs          # clean-room Lucas-Kanade pyramidal optical-flow tracker
      estimator.rs     # the Estimator trait + EstimatorOutput + EstimatorState
      estimators.rs    # the six modes + the registry
      scale.rs         # the rangefinder-free altitude ladder
      rangefinder.rs   # UART / I2C rangefinder drivers + the FC relay
      imu.rs           # IMU source (MAVLink RAW_IMU) + frame/IMU time aligner
      clock_align.rs   # bidirectional TIMESYNC clock alignment
      mavlink_emit.rs  # message builders + the component router
      pre_arm.rs       # the mode-aware pre-arm gate
      health.rs        # the navigation heartbeat snapshot
      vio.rs           # the VIO vendor-binary bridge (UDS control + SHM frame ring)
      pipeline.rs      # the per-frame runtime loop + plugin lifecycle
    calibration-helper/  # one-time offline camera-IMU calibration (Python, out of the hot path)
  vendor/                # OpenVINS + VINS-Fusion C++ adapters (binaries built by CI)
  gcs/                   # the GCS plugin half
  locales/               # translation strings

The estimator, the scale source, the rangefinder, and the IMU source each sit behind a Rust trait. Adding a new IMU source (DroneCAN over a USB-CAN adapter, for example) is a new type that implements the IMU contract. The one-time camera-IMU calibration stays Python. It is a heavyweight, infrequent, OpenCV-bound flow that runs once per camera mount, far off the 30 Hz pose-emit path, so it is not on the Rust hot path. It lives at calibration-helper/ and produces a Kalibr-style camchain.yaml that the Rust agent reads at start-up.

The EstimatorOutput contract

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

pub struct EstimatorOutput {
    pub timestamp_us: i64,
    pub output_mode: OutputMode,   // OpticalFlow | Vio | None
    pub state: EstimatorState,     // off|init|converging|converged|degraded|failed
    // OF path (None for VIO and off)
    pub flow_rate_x: Option<f32>,
    pub flow_rate_y: Option<f32>,
    pub flow_rate_z: Option<f32>,
    pub flow_quality: Option<i32>,
    pub flow_distance_m: Option<f32>,
    pub flow_scale_source: Option<ScaleSourceLabel>, // rangefinder|baro|gps|vision
    pub integration_time_us: Option<i64>,
    // VIO path (None for OF and off)
    pub pose: Option<(f32, f32, f32, f32, f32, f32)>, // (x,y,z,roll,pitch,yaw)
    pub velocity: Option<(f32, f32, f32)>,
    pub covariance: Option<Vec<f32>>,                 // 21-element upper-triangular
    pub feature_count: Option<i32>,
    pub reset_counter: Option<u32>,
    // Hybrid co-emit: an OF sample to also fire on comp 198
    pub extras_of: Option<Box<EstimatorOutput>>,
}

An optical-flow sample fills the flow_* fields and leaves the VIO fields None; a VIO sample fills pose / velocity / covariance and leaves the flow fields None. The component router reads output_mode and dispatches to either the OF emitter (component 198, OPTICAL_FLOW_RAD) or the VIO emitter (component 197, VISION_POSITION_ESTIMATE). A hybrid tick carries a co-emitted OF sample in extras_of so one tick fires on both components.

Sequence: a frame becomes a MAVLink message

The simplified flow for one frame in optical_flow mode:

1. The agent's vision host publishes the frame on the shared vision
   frame bus. The plugin's frame subscription receives it on the IPC
   reader task and forwards it down a channel (the callback never
   blocks).
2. framing::frame_to_gray converts the frame (nv12 / yuv420p / rgb24)
   to a packed grayscale image.
3. The worker pairs (prev_gray, curr_gray, dt) and looks up the
   closest IMU sample via imu::TimeAligner (gyro only for OF).
4. estimators::OpticalFlowEstimator::step runs flow::OpticalFlowLk and
   produces an EstimatorOutput.
5. mavlink_emit::ComponentRouter::emit routes the OF sample to
   build_optical_flow_rad on component 198 over the host MAVLink path.
6. health::HealthSnapshot refreshes with flow quality, rate, distance,
   and estimator state; the 1 Hz health tick publishes the navigation
   block.
7. The cloud relay forwards the heartbeat to Mission Control; the
   navigation card refreshes.

In vio_openvins mode the path changes at step 4: the worker bridges the frame into the VIO engine’s shared-memory ring and hands IMU samples to the engine over its UDS control channel. The vendor binary returns pose messages asynchronously; the estimator drains them per tick and fills the pose six-tuple. The router emits on component 197 instead of 198.

Calibration (separate Python helper)

The Rust agent does not run calibration in its hot loop. The one-time camera-IMU calibration is a separate Python package at calibration-helper/ (altnautica_vision_nav_calib) with three modules: intrinsics (Kalibr cam0 intrinsics loader and validator), extrinsics (the T_cam_imu plus timeshift loader and validator), and runner (the wizard coroutine). The runner decodes the captured frame bundle the GCS sends, runs OpenCV AprilTag detection (cv2.aruco, AprilGrid t36h11), solves the pinhole plus radial-tangential intrinsics with cv2.calibrateCamera, and fits the camera-IMU timeshift with a golden-section search over [-200 ms, +200 ms]. It writes a Kalibr-compatible camchain.yaml (the cam0 block: camera_model, intrinsics, distortion_model, distortion_coeffs, resolution, T_cam_imu, timeshift_cam_imu). The maths is in Calibration math. The Rust agent loads camchain.yaml at start-up. VIO modes feed it to the vendor estimator and to the time aligner. A calibration uploaded directly as YAML skips the wizard: the agent validates the file, persists it, and applies the new timeshift.

The scale ladder

scale::ScaleLadder runs only in optical_flow_degraded mode, where the tracker has no dedicated rangefinder. It walks four rungs on every pick() and returns the first healthy one, falling back to a static value so the estimator can always emit at the lowest quality rather than refuse to feed the EKF:

                                      ┌─ fresh ─> ScalePick(quality x0.7)
GLOBAL_POSITION_INT.relative_alt ─────┤
(<= 2 s old)                          └─ stale ─┐
                                                ▼
                                      ┌─ fresh ─> ScalePick(quality x0.6)
VFR_HUD.alt minus take-off ───────────┤
(<= 2 s old)                          └─ stale ─┐
                                                ▼
GPS_RAW_INT.alt minus take-off,       ┌─ healthy ─> ScalePick(quality x0.4)
outdoor flag + 3D fix + eph <= 200 cm ┤
                                      └─ unhealthy
                                                ▼
                                       ScalePick(quality x0.2) static
                                       fallback @ 1.5 m

Distance is clamped to [0.3, 50.0] m so a glitch reading cannot produce a runaway scale. The estimator multiplies the OF tracker’s raw quality by the rung’s multiplier before emitting, so the EKF auto-de-weights degraded rungs. The estimator marks itself degraded when the static rung is active so the GCS surfaces the warning banner. A new scale source plugs in behind the same contract; the estimator does not need to know which rung produced the number.

The rangefinder drivers

rangefinder.rs supports four sources behind one trait:

fc_relay is the default and universal path. The flight controller already publishes DISTANCE_SENSOR; the plugin relays the latest reading. No extra wiring.
tfluna_uart is a fully implemented Benewake TF-Luna UART driver (a pure 9-byte frame parser).
garmin_lidarlite_i2c and vl53l1x_i2c are documented stubs that fail safe (they return no reading) until the plugin SDK exposes an I2C facade. Wire an I2C sensor to the flight controller and use the fc_relay path instead.

The VIO bridge and the vendor binaries

The VIO modes spawn an out-of-process vendor binary through the plugin SDK’s process.spawn allowlist. The plugin host’s subprocess sandbox checks that the basename is declared in the manifest’s subprocess_spawn list (ados_openvins_shim, ados_vins_fusion_shim) and that the binary lives under <install_dir>/vendor/. CI builds the binaries on tag push and attaches the signed tarballs to the release; the install path unpacks them under the plugin’s cgroup slice. They stay on disk and never execute unless a VIO mode is selected.

+---------------------+   SHM ring (frames)    +---------------------+
| vision-nav (Rust)   | ────────────────────>  | ados_openvins_shim  |
|                     |   UDS msgpack (IMU,    | (C++ + libov_msckf) |
|                     |   pose, config, alive) |                     |
|                     | <────────────────────  |                     |
+---------------------+                        +---------------------+
        ▲                                              │
        │ EstimatorOutput                              │ libov_msckf
        ▼                                              ▼
  component_router                                VioManager loop
  -> comp 197                                     (MSCKF state)

The Rust plugin no longer captures frames. It bridges the shared vision-bus frames into the SHM ring the engine owns, then notifies the binary over the control channel. vio::VioEngine owns one spawned binary and the two channels it speaks: a POSIX shared-memory ring for camera frames (8 slots, the binary opens it read-only and reads the highest-sequence slot) and a length-prefixed msgpack control channel over a Unix-domain socket (hello / config / imu / frame_ready out; hello_ack / pose / alive / log in). The byte layouts match the vendored C++ adapters exactly so the binaries run unchanged. A missed alive heartbeat tears the engine down; a pose that fails to decode is dropped. Adding a third engine (a future home-grown estimator, for example) is a new VioEngine variant plus a vendor binary that speaks the same protocol.

The pre-arm gate

pre_arm::PreArmGate::evaluate() is a pure function over its inputs. It produces a report with a list of individual checks, each carrying a severity (ok / pending / blocking) and an operator-readable detail string. The aggregate armable flag is true only when every check is ok. The gate is mode-aware. Adding a new mode means:

Adding the mode key to the registry in estimators.rs.
Adding the mode variant to config.rs (Mode).
Adding a branch to PreArmGate::evaluate() that picks the right check set for the new mode.
Adding the mode label to the GCS mode card and the drone-card pill.

The gate does not call the network, does not touch the filesystem, and never blocks. The pipeline invokes it on the same tick that publishes the snapshot to the cloud relay.

How a heartbeat is built

Every health tick, health::HealthSnapshot::to_value() returns the navigation block (all keys camelCase; field names that drift to snake_case silently drop at the relay):

{
  "opticalFlowSupported": true,
  "vioSupported": true,
  "rangefinderTopology": "fc",
  "recommendedCameraId": "/dev/video0",
  "flowQuality": 180,
  "flowRateHz": 29.5,
  "flowDistanceM": 1.25,
  "vioState": "absent",
  "vioResetCounter": 0,
  "vioQuality": null,
  "companionState": "active",
  "mode": "optical_flow",
  "availableEstimators": [
    "hybrid_of_plus_vio",
    "off",
    "optical_flow",
    "optical_flow_degraded",
    "vio_openvins",
    "vio_vins_fusion"
  ],
  "estimatorState": "converged",
  "flowScaleSource": "rangefinder",
  "imuSource": "mavlink-raw-imu",
  "imuRateHz": 100,
  "cameraImuSyncOffsetMs": 4.2,
  "cameraIntrinsicsLoaded": true,
  "estimatorFeatureCount": null,
  "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"}
    ]
  }
}

This rides the cloud relay’s cmd_droneStatus.navigation field. The Mission Control normaliser reads it and routes it into the per-drone capability store. Every UI surface (the mode card, the sensors card, the estimator card, the fallback banner, the source-set switcher, the pre-arm status, the drone-card pill, and the fleet GPS-denied count) reads from that store.

Adding a new estimator

The full recipe:

Implement the Estimator trait in estimators.rs (or a new module): estimator_id, output_mode, step, plus configure / shutdown if it backs a subprocess.
Register the id in available_estimators() so the heartbeat advertises it and the GCS mode picker offers it.
Add the mode variant to Mode in config.rs with its wire string.
Add a branch to PreArmGate::evaluate() listing the pre-arm checks the new estimator needs.
Update the GCS: add the mode to the mode card with a description and hardware-requirements string, add it to the GCS mode type, and add the drone-card pill label.
Write tests. The existing Rust unit tests cover the contract; the new estimator’s tests verify its state machine and output shape.

Doing this for a stereo VIO engine, a learned monocular depth network, or a vendor MSCKF implementation is the same recipe each time. The pipeline does not change.

​Architecture

​Module map (agent half)

​The EstimatorOutput contract

​Sequence: a frame becomes a MAVLink message

​Calibration (separate Python helper)

​The scale ladder

​The rangefinder drivers

​The VIO bridge and the vendor binaries

​The pre-arm gate

​How a heartbeat is built

​Adding a new estimator