CMD 43003 — канал коррекций

CMD 43003 — это стандартная MAVLink-команда MAV_CMD_EXTERNAL_POSITION_ESTIMATE. Не наш custom, а upstream-feature ArduPilot. В коде существует с 4.5.0-beta1 (январь 2024).

modules/mavlink/message_definitions/v1.0/ardupilotmega.xml:333
<entry value="43003" name="MAV_CMD_EXTERNAL_POSITION_ESTIMATE">

Раньше у нас (в production 4.5) она была в прошивке, но не использовалась — наземка слала позицию через GPS_INPUT (см. ниже). С перехода на 4.6.3 V1.2.0 — это основной канал.

Формат сообщения — COMMAND_INT

⚠️ Важно: это COMMAND_INT, не COMMAND_LONG. Поля разные.

Поле	Тип	Что значит
target_system	uint8	Sysid борта (обычно 1)
target_component	uint8	Component id (обычно 1)
command	uint16	43003
frame	uint8	MAV_FRAME_GLOBAL или MAV_FRAME_GLOBAL_INT
param1	float	Timestamp измерения, секунды в формате timestamp_usec * 1e-6
param2	float	Processing time (задержка между измерением и отправкой), секунды
param3	float	Pos accuracy (метры) — используется для widening R_OBS в Zholobov latch
param4	float	(не используется)
x	int32	Latitude × 1e7
y	int32	Longitude × 1e7
z	float	Должно быть NaN (only XY supported)

Источник: libraries/GCS_MAVLink/GCS_Common.cpp:5305-5333.

MAV_RESULT GCS_MAVLINK::handle_command_int_external_position_estimate(
    const mavlink_command_int_t &packet)
{
    if ((packet.frame != MAV_FRAME_GLOBAL && packet.frame != MAV_FRAME_GLOBAL_INT) ||
        !isnan(packet.z)) {
        return MAV_RESULT_DENIED;       // ← frame должен быть GLOBAL, z должен быть NaN
    }
    ...
    uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(
        uint64_t(p2.param1 * 1e6), PAYLOAD_SIZE(chan, COMMAND_INT));
    const uint32_t processing_ms = p2.param2 * 1e3;
    const float pos_accuracy = p2.param3;
    if (timestamp_ms > processing_ms) {
        timestamp_ms -= processing_ms;
    }
    if (!AP::ahrs().handle_external_position_estimate(loc, pos_accuracy, timestamp_ms)) {
        return MAV_RESULT_FAILED;
    }
    return MAV_RESULT_ACCEPTED;
}

Путь в коде

Наземная система → MAVLink COMMAND_INT (id=43003)
   ↓
GCS_Common.cpp:5528 — handle_command_int (switch case MAV_CMD_EXTERNAL_POSITION_ESTIMATE)
   ↓
GCS_Common.cpp:5305 — handle_command_int_external_position_estimate(packet)
   ↓
AP_AHRS.cpp:1535   — AP_AHRS::handle_external_position_estimate(loc, pos_acc, ts)
   ↓
AP_AHRS.cpp:1538   — EKF3.setLatLng(loc, pos_acc, ts)
   ↓
AP_NavEKF3_PosVelFusion.cpp:198 — NavEKF3_core::setLatLng()
   ↓ (если EK3_SRC1_POSXY ∈ {EXTNAV, BEACON})
applyExtNavSoftCorrection() — наш Zholobov latch
   ↓ (первый вызов после boot)        ↓ (повторные вызовы)
ResetPositionNE (snap)              FuseVelPosNED (soft)

Гарды на пути

1. validOrigin (PosVelFusion.cpp:200-202)

if (!validOrigin) {
    return false;        // ← молча отвергается без origin
}

⇒ Перед первым CMD 43003 обязателен SET_GPS_GLOBAL_ORIGIN.

2. Source-dispatch (PosVelFusion.cpp:209-217)

if (posxy_source == EXTNAV || posxy_source == BEACON) {
    return applyExtNavSoftCorrection(loc, posAccuracy, timestamp_ms);
}

⇒ Soft-correction путь активен только при EK3_SRC1_POSXY ∈ {4, 6}. На GPS-конфигурации (SRC=3) идёт legacy hard-snap с V1.2.0 guard'ом.

3. Legacy V1.2.0 guard (PosVelFusion.cpp:224-227)

if ((imuSampleTime_ms - lastGpsPosPassTime_ms) < frontend->deadReckonDeclare_ms ||
    (PV_AidingMode == AID_NONE)) {
    return false;
}

Срабатывает только на не-EXTNAV/BEACON источниках. Защита от случайной CMD 43003 при healthy GPS-fix.

Что было раньше — GpsInject через GPS_INPUT

В Valkyrie 4.5 production-pipeline:

Наземная система → MAVLink GPS_INPUT (msg id 232)
   ↓
AP_GPS_MAV (драйвер с GPS_TYPE=14)
   ↓
AP_GPS — публикует данные как «обычный GPS-fix»
   ↓
EKF3 при EK3_SRC1_POSXY=3 (GPS) — стандартная GPS-fusion ветка
   ↓
ResetPositionNE при первом fix-е → задаёт origin
ResetPositionNE при больших innovation → snap каждый раз

GPS_INPUT несёт больше данных, чем CMD 43003:

Поле GPS_INPUT	Значение	В CMD 43003
lat, lon, alt	позиция	есть (только lat/lon)
vn, ve, vd	velocity NED	нет
speed_accuracy	velocity accuracy	нет
horiz_accuracy	pos accuracy	есть (param3)
vert_accuracy	vertical pos accuracy	нет (z=NaN)
hdop, vdop	dilution of precision	нет
fix_type	2D/3D/RTK	нет
satellites_visible	число спутников	нет

Поэтому EKF в 4.5 имел и позицию, и скорость от внешнего источника. В 4.6.3 — только позицию; скорость EKF выводит из IMU + airspeed.

Зачем переходили с GpsInject на CMD 43003

Архитектурно вынужденное решение:

• Soft-correction (Zholobov latch) реализован в функции setLatLng().
• GPS_INPUT идёт через GPS-fusion ветку EKF, не через setLatLng.
• Чтобы наш латч работал, коррекции должны попасть в setLatLng.
• Единственный стандартный способ — MAV_CMD_EXTERNAL_POSITION_ESTIMATE (CMD 43003).

Дополнительные плюсы CMD 43003:

• Per-call accuracy (param3 = pos_accuracy) — наземка явно говорит «эта коррекция точная на 5 метров» или «эта коррекция точная на 50 метров», и EKF учитывает через ширину R_OBS.
• Timestamp (param1) — учитывается задержка между измерением и доставкой.
• Прямой путь в EKF state — нет промежуточного GPS-драйвера с его таймаутами, blend-логикой и проверками.

Минусы:

• Только позиция, без velocity — EKF теряет один из источников скорости. Компенсируется airspeed-fusion + IMU.
• Origin не выставляется автоматически — нужно явное SET_GPS_GLOBAL_ORIGIN (см. origin).

Логирование

Каждый успешный setLatLng пишет запись в DAL:

// libraries/AP_NavEKF3/AP_NavEKF3.cpp:1435
dal.log_SetLatLng(loc, posAccuracy, timestamp_ms);

В .bin логе это будет видно как сообщение DAL — полезно для post-flight анализа: сколько коррекций пришло, какие.

Что не делает CMD 43003

• Не устанавливает origin. Это обязанность SET_GPS_GLOBAL_ORIGIN.
• Не корректирует высоту. Высота берётся из барометра (EK3_SRC1_POSZ = 1). Поле z в команде должно быть NaN.
• Не корректирует скорость. EKF выводит её из IMU + airspeed.
• Не корректирует yaw. Yaw берётся из компаса.
• Не путать с GPS_INJECT_DATA (msg id 233) — это RTCM-данные для физического GPS-приёмника, никак не связано с CMD 43003.