Vehicle Core¶

Firmware-agnostic flight logic shared by every MAVLink-class autopilot in the SDK. ArduPilot and PX4 are the same vehicle behavior reached through different firmware semantics and transports — all navigation, takeoff/land detection, GPS math, PID control, and movement API live here exactly once. Firmware specializations (ardupilot, px4) implement only the parts that differ; transports (mavros, mavlink) implement only the wire plumbing.

This README is the reference for the shared behavior — navigation methods, reference frames, altitude sources, takeoff/land detection, GPS/EGM96 handling, and PID configuration — and applies to every vehicle (ArduPilot and PX4 alike). Firmware-specific semantics are in ArduPilot (GUIDED, WPNAV/GUID_OPTIONS, native RTL params) and PX4 (OFFBOARD, mode names, RTL params).

Design¶

A two-axis bridge: a firmware-semantics class (VehicleDrone subclass) is composed with a wire-protocol VehicleTransport. The drone side owns flight orchestration on plain, ROS-free types (types.py); the transport side converts those to/from its wire format (ROS topics/services, raw MAVLink, or uXRCE-DDS). Identical flight logic therefore serves ArduPilot and PX4 over MAVROS or a direct link without duplication.

classDiagram
    class VehicleDrone {
        <<abstract>>
        -_transport VehicleTransport
        -_navigator VehicleNavigator
        -_sequencer FlightSequencer
        +takeoff() land() move_to() move_to_gps() move_velocity() rtl()
        +set_speed() set_actuator() set_gripper() set_setpoint_config()
        +arm()* _rtl_native()* _change_speed()* capabilities*
        +_command_takeoff() _command_land() _ensure_offboard_ready()
        +_emit_velocity() _prepare_position_setpoint() _load_firmware_config()
    }
    class ArduPilotDrone {
        GUIDED arm, RTL_ALT, GUID_OPTIONS/WPNAV
    }
    class Px4Drone {
        OFFBOARD + setpoint pump, AUTO.LAND/RTL, RTL_RETURN_ALT
    }
    class VehicleTransport {
        <<abstract>>
        +state local_pose vision_pose gps heading rel_alt rangefinder distance_sensors
        +arm() set_mode() command_takeoff() command_land() set_param()
        +send_velocity_target() send_local_target() send_global_target()
    }
    class MavrosTransport
    class PymavlinkTransport

    VehicleDrone <|-- ArduPilotDrone
    VehicleDrone <|-- Px4Drone
    VehicleDrone o-- VehicleTransport
    VehicleTransport <|.. MavrosTransport
    VehicleTransport <|.. PymavlinkTransport

Modules¶

Firmware hooks¶

VehicleDrone keeps all orchestration and delegates the firmware-specific pieces to overridable hooks:

Conventions¶

• Frames: the core is ENU (x=East, y=North, z=Up) / FLU; transports convert to/from the wire's NED/FRD.
• Yaw: radians, ENU (0 = East, CCW positive). Compass heading (degrees, NED) is kept separate for GPS body-frame math.
• Atomicity: telemetry properties return the most-recent value via whole-object assignment (atomic under the GIL), so the flight thread never sees a half-written pose.

Connection and Readiness¶

connect() does not return as soon as the transport starts — it waits (up to a timeout) for a live FCU heartbeat, setting _connected only once the link is up. After connecting:

• is_fcu_connected — FCU heartbeat present (raw link state).
• is_ready — connected and the driver/transport is running; this is the gate the higher-level calls check before arming or commanding.

Altitude Types¶

The FCU exposes several altitude definitions; the SDK selects between them with AltitudeSource:

AMSL vs Ellipsoid: GPS receivers output altitude above the WGS84 ellipsoid, but global setpoints expect AMSL. The difference is the geoid height, corrected by GPSUtils using the EGM96 model.

Surface Tracking: with a downward-facing rangefinder in range, the FCU can hold constant AGL (ArduPilot surface tracking). AltitudeSource.LIDAR implements a similar concept at the SDK level via PID control.

Coordinate Frames¶

The FCU uses NED (North-East-Down) / FRD (Forward-Right-Down) internally. The SDK core uses ENU (East-North-Up) / FLU (Forward-Left-Up). The transport performs the conversion on egress/ingest — SDK code always uses ENU/FLU.

The SDK's MoveReference enum maps to a wire TargetFrame:

Note: the EKF local frame needs an origin — outdoors GPS usually sets it; indoors without a GPS fix ArduPilot needs a manual (or 4.7+ recorded) origin before local pose / FRAME_LOCAL_NED commands work. Detail: Localization → EKF origin.

Distance Sensors¶

rangefinder / AltitudeSource.LIDAR exposes only the downward sensor used for altitude. Every rangefinder and proximity sector the FCU reports (one DISTANCE_SENSOR message per unit) is also available as raw telemetry, for obstacle detection, redundancy, or verification:

• drone.distance_sensors — dict[int, DistanceReading] keyed by MAVLink sensor id, holding the most recent reading per sensor.
• drone.get_distance(orientation) — the latest DistanceReading facing a given SensorOrientation, or None.

SensorOrientation mirrors the MAV_SENSOR_ORIENTATION values used for distance sensors: FORWARD, the yaw sectors (FORWARD_RIGHT, RIGHT, BACK_RIGHT, BACK, BACK_LEFT, LEFT, FORWARD_LEFT), UP, DOWN, and OTHER. Each DistanceReading carries distance, orientation, min_distance / max_distance, sensor_id, sensor_type (MAV_DISTANCE_SENSOR), signal_quality (when reported), raw_orientation, and a monotonic timestamp.

Sensor setup is on the FCU side (e.g. ArduPilot RNGFNDx_* rangefinder setup and, for horizontal sectors, PRXx_* proximity sensors). The downward sensor (SensorOrientation.DOWN) also updates rangefinder. The direct MAVLink transport collects all readings automatically; the MAVROS transport requires them declared in config (see the MAVROS README).

Takeoff and Landing¶

FlightSequencer owns climb/descent settle and airborne checks. Settle uses vertical velocity |dz/dt| over a short window so rangefinder/EKF/vision spikes (±0.2–0.3 m) do not reset detection. Absolute AGL alone is not used for settle, so body height or payload offsets do not change the logic. After velocity touchdown, land() waits for FCU landed confirmation before returning so a following takeoff() does not short-circuit while the vehicle is still on the ground.

Takeoff¶

drone.takeoff(altitude=1.5)  # defaults: max_retries=2, adjust_altitude=True, precision=0.12m, timeout=25s
drone.takeoff(altitude=2.0, adjust_altitude=False)

flowchart TD
  T0{is_airborne?} -->|yes| TSkip[return success]
  T0 -->|no| T1[arm]
  T1 --> T2[spin-up delay]
  T2 --> T3["_command_takeoff(alt)"]
  T3 --> T4[wait_takeoff_settle]
  T4 -->|settled| T5{adjust_altitude?}
  T5 -->|yes| T6["move_to(z, TAKEOFF)"]
  T5 -->|no| TOk[return success]
  T6 --> TOk
  T4 -->|timeout / disarm| TRetry[disarm and retry]

Per attempt: arm (firmware guided/offboard), _SPIN_UP_DELAY, capture takeoff pose on the first attempt only, _command_takeoff (FCU takeoff on ArduPilot; offboard climb setpoint on PX4), then wait_takeoff_settle. Optional post-settle move_to(z=altitude, reference=TAKEOFF) when adjust_altitude=True. Climb progress is logged ~1 Hz.

Settle requires all of: altitude rose by _LIFTOFF_DELTA; altitude ≥ target_alt - min(_SETTLE_ALT_TOLERANCE, _SETTLE_ALT_FRACTION × climb); mean |vz| over _SETTLE_WINDOW below _SETTLE_VELOCITY.

Retries: liftoff never seen (or mid-climb disarm) → disarm and retry; last attempt fails. Settled with height_gain < _LIFTOFF_DELTA while is_airborne is true is accepted (sensor glitch).

Airborne check (is_airborne)¶

Used by the takeoff short-circuit. Priority matches MAVSDK Telemetry::in_air (MAV_LANDED_STATE):

flowchart TD
  A1{disarmed?} -->|yes| ANo[false]
  A1 -->|no| A2{landed_state}
  A2 -->|ON_GROUND| ANo
  A2 -->|IN_AIR / TAKEOFF / LANDING| AYes[true]
  A2 -->|unknown| A3{system_status}
  A3 -->|STANDBY| ANo
  A3 -->|ACTIVE| A4{"rangefinder < threshold?"}
  A4 -->|yes| ANo
  A4 -->|no / none| AYes
  A3 -->|other| A5{rangefinder}
  A5 -->|above threshold| AYes
  A5 -->|else| ANo

MAVROS / direct MAVLink request EXTENDED_SYS_STATE via SET_MESSAGE_INTERVAL (ArduPilot does not stream it by default). Empty /mavros/extended_state usually means the stream was never requested. When landed_state is missing, only the rangefinder AGL gate is used — not vision or rel_alt.

Noisy lidar or a slow climb that never fully stops: raise _SETTLE_VELOCITY or shorten _SETTLE_WINDOW. Post-settle adjust still brings altitude within precision.

Land¶

drone.land()              # default timeout=60s
drone.land(timeout=45.0)

flowchart TD
  L1["_command_land()"]
  L1 --> L2[velocity touchdown]
  L2 --> L3{is_fcu_landed?}
  L3 -->|yes / disarmed| LOk[return success]
  L3 -->|timeout| LFail[return false]

1. Capture start_alt, send _command_land() (FCU land on ArduPilot, AUTO.LAND on PX4).
2. Velocity touchdown: descended (start_alt - alt > _LIFTOFF_DELTA or alt < _LANDED_THRESHOLD) and descent rate over _LAND_SETTLE_WINDOW below _LAND_STOP_VELOCITY, or disarmed.
3. FCU confirm (is_fcu_landed): landed_state == ON_GROUND, or system_status == STANDBY, or disarmed. Transports without MAVLink flight state treat touchdown as sufficient. Does not wait for autopilot DISARM_DELAY.

land() may return True while still armed (ON_GROUND / STANDBY during DISARM_DELAY). A later takeoff() is fine because is_airborne is false. Use drone.is_armed when the caller needs motors off.

Direct MAVLink arm/takeoff ACK behavior: MAVLink transport.

Navigation¶

Navigation lives in VehicleNavigator, keeping the drone classes focused on the firmware/hardware interface, sensor data, and target computation.

move_to and move_to_gps accept method: NavigationMethod (defaults: move_to → PID, move_to_gps → PID). rtl accepts method: RTLMethod (default NAVIGATE, which uses PID_EKF internally).

Capability Matrix¶

PX4 shares this matrix; the one deviation is POSITION_GLOBAL over the native uXRCE-DDS backend (px4_dds), where global setpoints are local-frame only — use a PID method or the MAVROS path (see PX4).

move_to Parameter Behavior¶

Axis values — x, y, z, yaw: each can be a float or None. The behavior depends on the method:

Key difference: with PID methods, None truly disables the axis (no velocity, excluded from the distance check). With POSITION methods the FCU receives a full 3D+yaw target and controls all axes; None means zero offset (a snapshot of the current value at call time).

Note on None accuracy: with PID, a disabled axis is uncontrolled — wind or inertia can cause drift with no correction. With POSITION, the target for a None axis is a snapshot of the current position, which may differ slightly from where you want to be. For precise multi-axis positioning, specify all axes explicitly.

Reference frames — reference:

With BODY, offsets chain from the current position. With TAKEOFF, offsets are absolute from the takeoff origin; None axes hold the current position on that axis (not the takeoff origin). move_to does not support WORLD and raises CapabilityNotSupportedError.

TAKEOFF reference, drone at (3, -2) in takeoff frame:

move_to(x=0, y=None) → target (0, -2)   # takeoff-origin X, current Y preserved
move_to(x=0, y=0)    → target (0, 0)    # full takeoff origin

Yaw + Position Behavior (PID / PID_EKF)¶

When yaw is specified together with a position axis (x or y), PID navigation is yaw-first:

1. Phase 1 — Yaw alignment: rotate to target yaw while holding position (zero translation). Completes when |dyaw| ≤ 3° (YAW_THRESHOLD).
2. Phase 2 — Translation: move to the target with yaw hold. Both x and y are activated regardless of which was specified, because after rotation the world-frame target may project onto either body axis.

The position target is computed at call time in the original heading direction — yaw rotation changes only the final orientation. With POSITION / POSITION_GLOBAL, yaw and position are controlled simultaneously by the FCU (no yaw-first phase).

Navigation Examples¶

Body-relative (relative to current position):

drone.move_to(x=2.0, y=0.0, z=0.0)            # 2m forward
drone.move_to(z=0.5)                           # 0.5m up (x/y disabled)
drone.move_to(x=3.0, yaw=45.0)                 # rotate 45°, then 3m to target

Takeoff-relative (absolute offsets from the takeoff origin):

drone.move_to(x=2.0, y=0.0, z=0.0, reference=MoveReference.TAKEOFF)
drone.move_to(x=0.0, y=0.0, z=0.0, reference=MoveReference.TAKEOFF)  # back to takeoff

Terrain following (z = height above ground):

drone.move_to(x=2.0, z=0.3, altitude_source=AltitudeSource.LIDAR)   # fly at 0.3m AGL

Navigation method (EKF local PID, or FCU position control):

drone.move_to(x=2.0, method=NavigationMethod.PID_EKF)
drone.move_to(x=2.0, y=1.0, method=NavigationMethod.POSITION)

GPS waypoints (outdoor):

drone.move_to_gps(latitude=-27.1234, longitude=-48.4567, altitude=15.0, precision=1.0)
drone.move_to_gps(latitude=-27.1234, longitude=-48.4567, altitude=15.0,
                  method=NavigationMethod.POSITION_GLOBAL)

Velocity:

drone.move_velocity(vx=0.5, reference=MoveReference.BODY)     # forward (heading-relative)
drone.move_velocity(vx=0.5, reference=MoveReference.WORLD)    # east (ENU absolute)
drone.move_velocity(vx=1.0, duration=2.0)                     # forward for 2s, then stop

Altitude Source Behavior¶

Altitude z parameter by reference:

LIDAR limit: capped at 15 m (LIDAR_ALTITUDE_LIMIT); falls back to position-based if exceeded.

The takeoff position is stored at the start of takeoff() (on the ground, before climbing). For vision/local frames takeoff_z ≈ 0, so with AUTO/VISION + TAKEOFF reference z is effectively the absolute height above the takeoff ground level. After takeoff(1.5), use z=1.5 to hold the same height — z=0 means "go to ground level".

Ground collision safety: move_to rejects z values that would produce a target altitude ≤ 0 (with TAKEOFF, z ≤ 0; with BODY, current_altitude + z ≤ 0). The axis is set to None (altitude disabled) and a warning is logged; the drone still moves on the other axes.

Navigation Flow¶

flowchart TD
    A["move_to / move_to_gps"] --> B{"Method?"}
    B -->|PID / PID_EKF| C["Compute PID target + resolve altitude target"]
    B -->|POSITION / POSITION_GLOBAL| D["Compute setpoint target"]
    C --> F["navigator.navigate_pid"]
    D --> G["navigator.navigate_setpoint"]
    F --> yawCheck{"yaw + position axes active?"}
    yawCheck -->|Yes| yawPhase["Phase 1: align yaw (zero translation)"]
    yawPhase --> yawDone{"|dyaw| ≤ 3°?"}
    yawDone -->|No| yawPhase
    yawDone -->|Yes| enableXY["Enable both x,y; reset yaw PID"]
    enableXY --> H["Phase 2: PID loop (body-frame errors)"]
    yawCheck -->|No| H
    H --> L["PID to move_velocity"]
    L --> conv{"distance ≤ precision AND |dyaw| ≤ 3°?"}
    conv -->|No| H
    conv -->|Yes| donePID["Target reached"]
    G --> I["Setpoint loop: publish_setpoint + check_reached"]
    I --> convSP{"reached AND |dyaw| ≤ 3°?"}
    convSP -->|No| I
    convSP -->|Yes| doneSP["Setpoint reached"]

PID Navigation¶

Velocity-based control with closed-loop feedback via navigate_pid():

1. The drone computes the target position (world frame, per reference) and resolves the altitude target (per altitude source).
2. The navigator creates per-axis PID controllers from pid_config.
3. If yaw + position: align yaw first (Phase 1), then enable both x and y.
4. Position loop (~100 Hz): compute the body-frame position and yaw errors, override the altitude error if an altitude target is set (LIDAR/REL_ALT), update the PIDs, publish velocity, and check arrival (distance ≤ precision AND |dyaw| ≤ 3°).

Dead zone: per-axis velocity is zeroed when |error| < precision / 2 to prevent oscillation. Active axes: only non-None axes are controlled and counted in the distance check (the yaw+x/y exception above applies).

Setpoint (Position) Navigation¶

Direct setpoint publishing via navigate_setpoint(). The FCU receives a full target (position + yaw) and controls all axes simultaneously — no yaw-first phase.

• Local (LocalTarget): publishes a local NED setpoint; checks Euclidean distance using the EKF local pose.
• Global (GlobalTarget): publishes a global AMSL setpoint; checks geodesic distance using GPS + relative altitude.

Both verify the target yaw is reached (within YAW_THRESHOLD = 3°) before declaring arrival.

How a firmware routes a published setpoint to its onboard controllers is firmware-specific: ArduPilot's GUIDED-mode AC_PosControl/AC_WPNav selection and WPNAV_* parameters are in ArduPilot; PX4's continuous OFFBOARD setpoint pump is in PX4.

Reference Frame Transformations¶

• BODY — relative to current position/orientation. move_to rotates the offset into world coordinates by the current yaw before adding it to the current position.
• WORLD — absolute ENU directions regardless of heading (move_velocity only; move_to raises CapabilityNotSupportedError). Works indoors (vision + EKF origin) and outdoors (GPS).
• TAKEOFF — relative to the takeoff position/orientation. Requires the takeoff position to be set via takeoff() or set_takeoff_position().

RTL¶

rtl() defaults to RTLMethod.NAVIGATE (SDK PID path to home). Use RTLMethod.NATIVE for the FCU's own RTL mode.

drone.rtl(altitude=5.0, method=RTLMethod.NAVIGATE, land=True)   # climb, fly to takeoff via PID, land
drone.rtl(method=RTLMethod.NATIVE)                              # FCU RTL, auto-land

• NAVIGATE: optionally climb/descend to altitude, navigate to the takeoff position (x=0, y=0, z=0, reference=TAKEOFF, PID_EKF), then land if land=True. This path is identical across firmwares.
• NATIVE: switches the FCU into its own return-to-launch mode; the mode name and altitude/loiter parameters are firmware-specific — see ArduPilot (RTL, RTL_ALT/RTL_ALT_FINAL) and PX4 (AUTO.RTL, RTL_RETURN_ALT/RTL_LAND_DELAY).

GPS Utilities¶

GPSUtils provides static methods for outdoor navigation, used by the navigator and drone.

EGM96 Geoid Correction¶

GPS altitude (WGS84 ellipsoid) differs from AMSL by the geoid height. Global setpoints expect AMSL. GPSUtils uses the EGM96 geoid model (5′ grid, cubic interpolation) to convert:

AMSL = GPS_ellipsoid_altitude - geoid_height + relative_altitude

Note: the EGM96 dataset must be installed (provided by GeographicLib, read from /usr/share/GeographicLib/geoids/egm96-5.pgm).

API¶

from nectar.control.vehicle.gps_utils import GPSUtils

GPSUtils.geoid_height(latitude, longitude)                     # EGM96 geoid height (m)

GPSUtils.create_global_target(                                 # AMSL-corrected GlobalTarget
    latitude, longitude, altitude_rel, heading, initial_altitude
)

reached, distance, alt_diff = GPSUtils.check_reached(          # geodesic arrival check
    cur_lat, cur_lon, cur_alt, tgt_lat, tgt_lon, tgt_alt,
    precision_radius=0.5, alt_threshold=0.5,
)

east, north = GPSUtils.local_offset(                           # equirectangular E/N offset (m)
    cur_lat, cur_lon, tgt_lat, tgt_lon,
)

create_global_target stores yaw in ENU radians (converted from the NED heading) to match the local-frame convention. local_offset is an equirectangular approximation of the east/north offset in meters; arrival is always decided by the geodesic distance from check_reached.

PID Configuration¶

VehicleDrone loads a PositionPIDConfig (per-axis x/y/z/yaw PIDConfig) at construction. If pid_config_file is set it is loaded from there; otherwise the bundled per-firmware presets (position_indoor.yaml / position_outdoor.yaml, under each firmware's config/ — ArduPilot presets or PX4 presets) are selected by is_indoor. SITL presets ship as position_sim_indoor.yaml / position_sim_outdoor.yaml — point pid_config_file at them when flying the simulator. Update at runtime from a file path, dict, or object:

drone.set_pid_config("/path/to/config.yaml")
drone.set_pid_config({"x": {"kp": 0.8, "output_min": -1.0, "output_max": 1.0}})

The controller internals (gains, output clamps, integral handling) live in the PID module.

Transports¶

The same vehicle is reached over interchangeable transports, all implementing the VehicleTransport ABC:

• MAVROS transport — MavrosTransport: subscriptions → telemetry, service clients → commands, publishers → setpoints. Requires a running mavros_node. Shared by ArduPilot and PX4.
• Direct MAVLink transport — PymavlinkTransport: owns the FCU link directly (RX timer decode, direct mav.*_send), built-in vision bridge. Firmware-neutral via an injected mode codec.
• PX4 uXRCE-DDS transport — Px4DdsTransport: native PX4 uORB over the uXRCE-DDS bridge (px4_msgs), no MAVROS.

References¶

• MAVLink common messages · MAV_FRAME · SET_POSITION_TARGET_LOCAL_NED
• Understanding Altitude · EGM96 · GeographicLib
• Firmware specifics: ArduPilot · PX4 · PID tuning: PID