SET_GPS_GLOBAL_ORIGIN¶

EKF3 хранит позицию относительно «origin» — точки в LLH-координатах, выбираемой при старте. Все NED-расчёты внутри EKF — относительно этого origin. На GPS-конфигурации origin устанавливается автоматически при первом 3D-fix. На нашей конфигурации (без GPS) — только вручную через MAVLink SET_GPS_GLOBAL_ORIGIN.

Почему обязателен¶

// libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp:200-202
bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
{
    if (!validOrigin) {
        return false;        // ← без origin CMD 43003 не работает
    }
    ...
}

Пока validOrigin = false, любая CMD 43003 молча отвергается.

Путь установки origin¶

GCS → MAVLink SET_GPS_GLOBAL_ORIGIN(lat, lng, alt)
  → GCS_MAVLINK::handle_set_gps_global_origin
    → set_ekf_origin(loc)
      → AHRS::set_origin(loc)
        → NavEKF3::setOriginLLH(loc)
          → for each core: core[i].setOriginLLH(loc)  →  validOrigin = true

Источник: libraries/GCS_MAVLink/GCS_Common.cpp:3790-3806, libraries/AP_NavEKF3/AP_NavEKF3.cpp:1411-1430.

Set-once природа¶

// libraries/AP_NavEKF3/AP_NavEKF3.cpp:1418-1423
if (common_origin_valid) {
    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3: origin already set");
    return false;
}

Origin устанавливается один раз за сессию. Повторный SET_GPS_GLOBAL_ORIGIN будет молча отвергнут с warning в GCS. Чтобы поменять — нужен reboot.

Что это значит на практике

Если ты ошибся с координатами в первом SET_GPS_GLOBAL_ORIGIN (например, ввёл координаты другой точки), исправить можно только перезагрузкой полётника. Это критично для процедуры — origin фиксирует «нулевую точку» NED-координат, относительно которой считаются все позиции, включая waypoint navigation в AUTO.

Что произойдёт без origin¶

Действие	Результат без origin
`SET_GPS_GLOBAL_ORIGIN`	устанавливает origin → `validOrigin=true`
Первая CMD 43003	❌ молча отбрасывается (`return false`)
pre-arm check	проходит (если `ARMING_CHECK` GPS bit выключен), но EKF в `AID_NONE`
arm	технически возможен, но в воздухе EKF не сможет навигировать
Waypoint navigation	❌ не работает (нет глобальной привязки)
RTL	❌ не работает (нет «home» в глобальных координатах)

⇒ Запустить борт без origin теоретически можно, но это будет полёт в MANUAL/FBWA без какой-либо навигации.

Поле altitude в SET_GPS_GLOBAL_ORIGIN¶

// GCS_Common.cpp:3801-3805
Location ekf_origin {};
ekf_origin.lat = packet.latitude;
ekf_origin.lng = packet.longitude;
ekf_origin.alt = packet.altitude / 10;     // mm в cm
set_ekf_origin(ekf_origin);

Альтитуда используется — это «нулевая высота» относительно которой EKF считает свой высотный state. На практике борт получит «высоту над origin», а абсолютная высота над уровнем моря вычисляется добавлением ekf_origin.alt.

Рекомендация: альтитуду origin задавать как высоту точки старта над уровнем моря (или над любой выбранной нулевой плоскостью, главное согласованно с GCS).

Acknowledgement¶

После успешного SET_GPS_GLOBAL_ORIGIN борт отправляет MAVLink GPS_GLOBAL_ORIGIN обратно. В Mission Planner / MAVProxy это видно в окне сообщений. Если ack не пришёл — origin не установлен.

// GCS_Common.cpp:3783-3787
if (!try_send_message(MSG_ORIGIN)) {
    send_message(MSG_ORIGIN);   // повторная попытка
}

Альтернативы (на будущее)¶

Текущая процедура требует двух отдельных MAVLink-сообщений (SET_GPS_GLOBAL_ORIGIN + первая CMD 43003). Можно упростить:

C++ патч: в applyExtNavSoftCorrection авто-сидинг origin при первом вызове если !validOrigin. Это новый код в EKF core, требует ревью.
Lua-скрипт: на борту скрипт ждёт первую CMD 43003 и сам отправляет origin через ahrs:set_origin() (требует проверки доступности binding'а). SCR_ENABLE=1 уже стоит в production-baseline.

Оба варианта — отложенные задачи. Для первых полётов используем явный двухшаговый протокол.