Приложение B — MAVLink команды¶

Справочник по командам, используемым в процедуре взлёта и в полёте.

SET_GPS_GLOBAL_ORIGIN¶

Стандартная MAVLink-команда (ID 48). Устанавливает origin EKF.

Поля¶

Поле	Тип	Описание
`target_system`	uint8	Sysid борта
`latitude`	int32	Широта × 1e7
`longitude`	int32	Долгота × 1e7
`altitude`	int32	Высота × 1000 (мм над уровнем моря)
`time_usec`	uint64	Timestamp (опционально)

Пример (MAVProxy)¶

set_origin 49.5912345 36.2456789 150.0

Пример (pymavlink)¶

mav.set_gps_global_origin_send(
    target_system=1,
    latitude=int(49.5912345 * 1e7),
    longitude=int(36.2456789 * 1e7),
    altitude=int(150.0 * 1000),     # mm
    time_usec=0
)

Ответ¶

Борт отправляет MAVLink GPS_GLOBAL_ORIGIN (ID 49) обратно при успехе. Если origin уже установлен — STATUSTEXT EKF3: origin already set, без ack.

CMD 43003 — `MAV_CMD_EXTERNAL_POSITION_ESTIMATE`¶

Стандартная MAVLink-команда из ArduPilot dialect (ardupilotmega.xml). В коде ArduPilot с 4.5.0-beta1 (январь 2024). Не наш custom.

⚠️ Формат — COMMAND_INT, не COMMAND_LONG.

Поля¶

Поле	Тип	Описание
`target_system`	uint8	Sysid борта
`target_component`	uint8	Component id
`command`	uint16	43003
`frame`	uint8	`MAV_FRAME_GLOBAL` (5) или `MAV_FRAME_GLOBAL_INT` (10)
`current`	uint8	0
`autocontinue`	uint8	0
`param1`	float	Timestamp измерения, секунды (`timestamp_usec * 1e-6`)
`param2`	float	Processing time (задержка измерение→отправка), секунды
`param3`	float	Pos accuracy, метры (важно — определяет ширину R в Zholobov latch)
`param4`	float	(не используется)
`x`	int32	Latitude × 1e7
`y`	int32	Longitude × 1e7
`z`	float	NaN (only XY supported, высоту даёт барометр)

Валидация на стороне борта (GCS_Common.cpp:5305): - Если frame ∉ {GLOBAL, GLOBAL_INT} → MAV_RESULT_DENIED. - Если z ≠ NaN → MAV_RESULT_DENIED.

Пример (pymavlink)¶

import math
from pymavlink import mavutil

# Координаты коррекции
LAT = int(49.5912345 * 1e7)
LON = int(36.2456789 * 1e7)
POS_ACCURACY_M = 5.0   # точность ExtNav-источника, метры
TIMESTAMP_S = 0.0      # 0 = «считай что измерение сейчас»

mav.mav.command_int_send(
    mav.target_system, mav.target_component,
    mavutil.mavlink.MAV_FRAME_GLOBAL,
    43003,                  # MAV_CMD_EXTERNAL_POSITION_ESTIMATE
    0, 0,                   # current, autocontinue
    TIMESTAMP_S,            # param1: timestamp (s)
    0.0,                    # param2: processing time (s)
    POS_ACCURACY_M,         # param3: pos accuracy (m)
    0.0,                    # param4: unused
    LAT, LON,               # x, y
    float('nan'),           # z = NaN (only XY)
)

Внутри борта (трасса вызовов)¶

COMMAND_INT (cmd=43003)
  → GCS_MAVLINK::handle_command_int (GCS_Common.cpp:5528)
    → handle_command_int_external_position_estimate (GCS_Common.cpp:5305)
      → AP::ahrs().handle_external_position_estimate (AP_AHRS.cpp:1535)
        → EKF3.setLatLng(loc, pos_accuracy, timestamp_ms) (AP_AHRS.cpp:1538)
          → NavEKF3_core::setLatLng (PosVelFusion.cpp:198)
            → applyExtNavSoftCorrection (если SRC=ExtNav)

См. tech/architecture/cmd-43003 для подробностей.

Ответ¶

MAVLink COMMAND_ACK с одним из:

MAV_RESULT_ACCEPTED — команда принята, setLatLng вернул true.
MAV_RESULT_DENIED — невалидный frame или z ≠ NaN.
MAV_RESULT_FAILED — setLatLng вернул false (нет origin, или EKF не готов).

Внимание: ACCEPTED не гарантирует что коррекция реально применилась (например, Zholobov latch может применить soft-fuse с очень узким Kalman gain, эффект будет слабый). Для надёжности следить за EKF_STATUS_REPORT.pos_horiz_variance (должен упасть после успешной коррекции).

EKF_STATUS_REPORT (получаем)¶

Стандартное MAVLink-сообщение (ID 193). Передаётся раз в секунду.

Поля¶

Поле	Тип	Описание
`flags`	uint16	Битмаска состояний EKF
`velocity_variance`	float	Безразмерная (5σ норма)
`pos_horiz_variance`	float	Безразмерная
`pos_vert_variance`	float	Безразмерная
`compass_variance`	float	Безразмерная
`terrain_alt_variance`	float	Безразмерная

Флаги — см. pilot/gcs.

Подписаться через MAVProxy¶

status EKF_STATUS_REPORT

Через pymavlink¶

mav.command_long_send(
    target_system, target_component,
    mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
    mavutil.mavlink.MAVLINK_MSG_ID_EKF_STATUS_REPORT,
    100000,    # 100 ms = 10 Hz
    0, 0, 0, 0, 0
)

GLOBAL_POSITION_INT (получаем)¶

Стандартное (ID 33). Текущая EKF-оценка позиции в LLH.

Поле	Тип	Описание
`lat`	int32	Широта × 1e7
`lon`	int32	Долгота × 1e7
`alt`	int32	Высота × 1000 (мм MSL)
`relative_alt`	int32	Высота относительно home × 1000
`vx, vy, vz`	int16	Скорость × 100 (см/с)
`hdg`	uint16	Heading × 100 (centidegrees)

VFR_HUD (получаем)¶

Стандартное (ID 74). Текущая airspeed, groundspeed, высота, heading, throttle, climb rate.

Поле	Описание
`airspeed`	m/s
`groundspeed`	m/s
`heading`	degrees
`throttle`	%
`alt`	m AMSL
`climb`	m/s

STATUSTEXT (получаем)¶

Стандартное (ID 253). Текстовые сообщения от борта (errors, warnings, info).

Severity	Значение
0	EMERGENCY
1	ALERT
2	CRITICAL
3	ERROR
4	WARNING
5	NOTICE
6	INFO
7	DEBUG

Список релевантных сообщений см. pilot/gcs.

Минимальный pymavlink-скрипт для процедуры¶

#!/usr/bin/env python3
"""Минимальный шаблон процедуры взлёта Variant B."""
from pymavlink import mavutil
import time

m = mavutil.mavlink_connection('/dev/ttyACM0', baud=115200)
m.wait_heartbeat()

LAT = int(49.5912345 * 1e7)
LON = int(36.2456789 * 1e7)
ALT_MSL_MM = int(150.0 * 1000)

# Шаг 2: SET_GPS_GLOBAL_ORIGIN
m.mav.set_gps_global_origin_send(m.target_system, LAT, LON, ALT_MSL_MM, 0)
print("Sent SET_GPS_GLOBAL_ORIGIN, waiting for ack...")

# Ждать GPS_GLOBAL_ORIGIN ack
ack = m.recv_match(type='GPS_GLOBAL_ORIGIN', blocking=True, timeout=5)
if not ack:
    print("FAIL: no GPS_GLOBAL_ORIGIN ack")
    raise SystemExit(1)
print(f"OK: origin set to {ack.latitude/1e7}, {ack.longitude/1e7}, {ack.altitude/1000} m")

time.sleep(1)

# Шаг 3: Первая CMD 43003 (MAV_CMD_EXTERNAL_POSITION_ESTIMATE, COMMAND_INT)
m.mav.command_int_send(
    m.target_system, m.target_component,
    mavutil.mavlink.MAV_FRAME_GLOBAL,
    43003,                  # MAV_CMD_EXTERNAL_POSITION_ESTIMATE
    0, 0,
    0.0,                    # param1: timestamp (s); 0 = «сейчас»
    0.0,                    # param2: processing time (s)
    5.0,                    # param3: pos accuracy (m)
    0.0,                    # param4: unused
    LAT, LON,               # x, y
    float('nan'),           # z = NaN
)
print("Sent CMD 43003 (first correction)")

# Подождать EKF в POS_HORIZ_ABS
deadline = time.time() + 5
while time.time() < deadline:
    s = m.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=1)
    if s and (s.flags & 16):
        print(f"OK: EKF in POS_HORIZ_ABS, flags=0x{s.flags:04x}")
        break
else:
    print("FAIL: EKF не в POS_HORIZ_ABS")
    raise SystemExit(2)

print("Ready to arm.")

Приложение B — MAVLink команды¶

SET_GPS_GLOBAL_ORIGIN¶

Поля¶

Пример (MAVProxy)¶

Пример (pymavlink)¶

Ответ¶

CMD 43003 — MAV_CMD_EXTERNAL_POSITION_ESTIMATE¶

Поля¶

Пример (pymavlink)¶

Внутри борта (трасса вызовов)¶

Ответ¶

EKF_STATUS_REPORT (получаем)¶

Поля¶

Подписаться через MAVProxy¶

Через pymavlink¶

GLOBAL_POSITION_INT (получаем)¶

VFR_HUD (получаем)¶

STATUSTEXT (получаем)¶

Минимальный pymavlink-скрипт для процедуры¶

CMD 43003 — `MAV_CMD_EXTERNAL_POSITION_ESTIMATE`¶