MAVLINKHUD

Overview

The VISO parameter group configures the Visual Odometry (External Navigation) subsystem. This allows ArduPilot to use position and velocity data from an external camera-based system (like Intel RealSense or an OptiTrack mo-cap system) for navigation.

Visual Odometry is the primary method for high-performance non-GPS autonomous flight.

Key Concepts

1. Vision Types (VISO_TYPE)

  • 1 (MAVLink): Receives VISION_POSITION_ESTIMATE or ODOMETRY MAVLink messages from a companion computer.
  • 2 (Intel RealSense): Specialized driver for RealSense devices.

2. Offsets and Orientation

  • VISO_POS_X/Y/Z: The physical distance from the camera lens to the vehicle's center of gravity.
  • VISO_ORIENT: The rotation of the camera relative to the flight controller.

Parameter Breakdown

  • VISO_ENABLE: Master switch.
  • VISO_DELAY_MS: Time lag of the vision system (crucial for EKF synchronization).
  • VISO_QUAL_MIN: Minimum confidence threshold for the vision solution.

Integration Guide

  1. Preparation: Requires an H7 processor and a high-speed telemetry link (921k baud).
  2. Config: Set VISO_TYPE = 1.
  3. EKF Source: Set EK3_SRC1_POSXY = 6 (External Nav).
  4. Verify: Check the GCS map. The drone's position should move accurately as you walk it around a room.

Developer Notes

  • Library: libraries/AP_VisualOdom.
  • Messages: Primarily consumes the MAVLINK_MSG_ID_ODOMETRY message.

VISO_DELAY_MS

ms
Default 10
Range 0 250

Visual Odometry Processing Delay (VISO_DELAY_MS)

Description

VISO_DELAY_MS tells the autopilot how "Old" the vision-based position data is when it arrives.

Vision algorithms (like those in the Intel Realsense T265) take significant time to process images and calculate a pose. If the EKF (Extended Kalman Filter) thinks this data is happening "Now," but it actually represents where the drone was 50ms ago, the position estimate will be unstable. This parameter allows the autopilot to look back in its IMU history and align the vision data with the correct moment in time.

Tuning & Behavior

  • Default: 10 ms.
  • Recommendation: Consult your vision system's documentation for its typical latency. For the T265, 50ms to 100ms is common.
  • Symptom of incorrect delay: The drone may "Wobble" or circle slowly while trying to hold position, especially after a quick movement.

VISO_ORIENT

Default 0
Range 0 35

Visual Odometry Orientation (VISO_ORIENT)

Description

VISO_ORIENT defines the mounting angle of your Visual Inertial Odometry (VIO) camera.

For the autopilot to correctly translate "Camera Motion" into "Vehicle Motion," it must know which way the camera is facing.

  • 0: Rotation_None (Standard). Camera is facing forward, right-side up.
  • Other Values: Standard ArduPilot rotations (e.g. 25 for Downward).

Tuning & Behavior

  • Default: 0.
  • Reboot Required: Yes.
  • Validation: View the VISO position data in your GCS while moving the drone. If moving forward causes the reported position to move backward or sideways, this orientation parameter is likely incorrect.

VISO_POS

m
Default 0
Range -5 5

Visual Odometry Position Offset (VISO_POS)

Description

VISO_POS (often appearing as VISO_POS_X, VISO_POS_Y, VISO_POS_Z) defines the physical location of the camera lens relative to the drone's center of gravity (CoG).

Correctly setting these offsets allows the autopilot to perform "Lever Arm Compensation." This ensures that when the drone rotates (pitches or rolls), the resulting camera movement isn't mistaken for actual horizontal translation.

  • X: Positive = Forward of CoG.
  • Y: Positive = Right of CoG.
  • Z: Positive = Below CoG.

Tuning & Behavior

  • Default: 0.
  • Measurement: Use a tape measure to find the distance from the center of the flight controller (or the vehicle's balance point) to the center of the camera lenses.
  • Accuracy: Small errors (1-2cm) are usually acceptable, but errors > 5cm can cause instability in position hold during aggressive maneuvers.

VISO_POS_M_NSE

m
Default 0.2
Range 0.1 10.0

Visual Odometry Position Noise (VISO_POS_M_NSE)

Description

VISO_POS_M_NSE tells the EKF how much to "Trust" the position data from your vision sensor.

If the vision sensor reports a noise value that is lower than this parameter, the autopilot overrides it with this value. This ensures the navigation filter remains conservative and doesn't over-react to potentially over-confident vision data, preventing the drone from "twitching" in high-gain vision scenarios.

Tuning & Behavior

  • Default: 0.2 m.
  • Recommendation: Leave at 0.2 for standard Intel T265 setups.

VISO_QUAL_MIN

%
Default 0
Range -1 100

Visual Odometry Minimum Quality (VISO_QUAL_MIN)

Description

VISO_QUAL_MIN defines the "Certainty Threshold" for your vision-based position data.

Computer vision systems (like the T265) report a "Confidence" score along with their position. In low light or over featureless surfaces (like smooth white floors), the system may become "lost" and start reporting random or drift-prone data. This parameter tells the autopilot to reject any vision data if the confidence falls below this level.

  • 0 (Default): Use data if the sensor thinks it's okay.
  • 50: Reject data if the sensor is less than 50% sure.
  • -1: Trust all data, even if the sensor says it is failed (DANGEROUS).

Tuning & Behavior

  • Recommendation: Set to 50 or higher for safety. If the vision system fails this quality check, the autopilot will fall back to GPS (if available) or trigger an EKF failsafe.

VISO_SCALE

Default 1.0
Range 0.1 2.0

Visual Odometry Scale Factor (VISO_SCALE)

Description

VISO_SCALE allows you to calibrate the "Ruler" used by your vision sensor.

Sometimes, due to camera lens distortion or inaccuracies in the vision algorithm, the sensor might think it has moved 1.0 meters when it has actually moved 1.1 meters. This scaling factor allows you to correct that error so that simulated movement matches real-world distance.

  • 1.0 (Default): No scaling.
  • 1.1: Increases the reported distance by 10%.

Tuning & Behavior

  • Calibration: Place the drone on a long measuring tape. Move it manually from 0 to 5 meters. Observe the reported VISO position in the GCS.
    • $$ \text{Scale} = \frac{\text{Actual Distance}}{\text{Reported Distance}} $$
  • Note: If your scale factor is far from 1.0 (e.g. 1.5 or 0.5), it usually indicates a more fundamental problem with the vision sensor's calibration or lens FOV settings.

VISO_TYPE

Default 0
Range 0 3

VISO_TYPE: Visual Odometry Type

Description

Selects the visual odometry camera driver.

Values

Value Meaning
0 Disabled
1 MAVLink (OpenMV, T265 via MAVLink)
2 Intel T265 (USB) - Linux boards only
3 VOXL (Serial)
  • Default: 0

Description

This parameter enables the Visual Odometry subsystem, allowing the drone to hold position and navigate without GPS by tracking features in the environment.

  • MAVLink (1): Most common. Used when a companion computer (like a Raspberry Pi or Jetson) runs the VIO software (e.g., OpenVINS, ORB-SLAM) and sends the pose data to the flight controller via MAVLink.
  • T265 (2): Only applicable for Linux-based flight controllers that can directly read the Intel Realsense T265 via USB.

VISO_VEL_M_NSE

m/s
Default 0.1
Range 0.05 2.0

Visual Odometry Velocity Noise (VISO_VEL_M_NSE)

Description

VISO_VEL_M_NSE tells the EKF how much to trust the velocity (speed) reports from your vision sensor.

VISO_YAW_M_NSE

rad
Default 0.2
Range 0.05 1.0

Visual Odometry Yaw Noise (VISO_YAW_M_NSE)

Description

VISO_YAW_M_NSE defines how much the autopilot trusts the heading data coming from a Visual Odometry (VO) sensor (like an Intel T265).

  • Low Value: High trust. The EKF will rely heavily on the camera for heading.
  • High Value: Low trust. The EKF will rely more on the IMU and Compass.

Tuning & Behavior

  • Default Value: 0.2 rad.
  • Recommendation: If the drone's heading "Glitchy" while using VO, increase this value to reduce the camera's influence.