AltHold, PosHold, Loiter: архитектура ArduPilot

Краткое сравнение

Режим	Горизонталь	Вертикаль	Управление стиками	Требует GPS	Когда использовать
AltHold	Ручной (Stabilize)	Авто	Roll/Pitch → угол, Throttle → скорость набора	Нет	Полёт без GPS, манёвры в помещении, взлёт/посадка
PosHold	Позиция	Авто	Roll/Pitch → угол наклона, отпускание → торможение углом	Да	Точное позиционирование, работа вблизи препятствий
Loiter	Позиция	Авто	Roll/Pitch → желаемая скорость, отпускание → плавный стоп	Да	Автономные миссии, ретрансляторы, стабильный холд

📌 Рекомендация: для большинства задач Loiter предпочтительнее — предсказуемее и плавнее.

Единая архитектура (все 3 режима)

┌─────────────────────────────────────────────────────────┐
│                                                                   СЕНСОРЫ                                                                       │
│                IMU (1000Hz)                                       │        GPS (5-20Hz)     │ Baro (50Hz) │ Rangefinder│
└────────┬────────────────────┴─────────────┬────────────┘
                          │                                                      │
                         ▼                                                     ▼
┌─────────────────┐              ┌────────────────┐
│              EKF3 @ 50Hz          │              │  RC INPUT @ 50-490Hz   │
│ • State: pos/vel/att             │                        • Mode select            │
│ • Fusion: GPS+IMU+Baro                   │       • Stick mapping        │
└────────┬────────┘              └────────┬───────┘
                         │                                                               │
                        ▼                                                              ▼
┌─────────────────────────────────   ─┐
│              КОНТРОЛЛЕР ВЫСОТЫ (общий)                       │
│  ┌─────────────────────── ────────┐   │
│  │  AltHold / Loiter / PosHold используют один     │   │
│  │                      каскад для Z-оси:                                │   │
│  │                                                                                      │   │
│  │ Alt Error → PSC_POSZ_P → Target Climb Rate    │   │
│  │     Climb Rate Error → PSC_VELZ_P → Accel         │   │
│  │     Accel Error → PSC_ACCZ_P/I/D → Throttle      │    │
│  └───────────────────────────────┘    │
└───────────────────┬──────────────┘
                                                      │
                     ┌───────────┴─────────┐
                     ▼                                                       ▼
┌─────────────────┐  ┌─────────────────┐
│                      ALTHOLD:       │  │  LOITER / POSHOLD:         │
│           • XY = Stabilize          │  │  • AC_PosControl @10Hz   │
│           • Нет GPS-контура   │  │                                               │
│                                               │  │  ┌──────────────┐  │
└─────────────────┘  │  │       Horizontal Path:   │  │
                                                     │  │                                       │  │
                                                     │  │         Loiter:                   │  │
                                                    │  │         pos_error ×          │  │
                                                    │  │         PSC_POSXY_P → │  │
                                                    │  │         target_vel            │  │
                                                    │  │                                       │  │
                                                    │  │           PosHold:            │  │
                                                    │  │         stick → angle      │  │
                                                    │  │         angle → vel via   │  │
                                                    │  │         physics                 │  │
                                                    │  └──────────────┘  │
                                                    └────────┬────────┘
                                                                            ▼
                                                     ┌────────────┐
                                                     │      Velocity PID:    │
                                                     │        vel_error ×     │
                                                    │ PSC_VELXY_P/I/D │
                                                    │ → target_accel     │
                                                    │ → target_angle    │
                                                    └─────┬──────┘
                                                                    ▼
                                                    ┌─────────────┐
                                                    │  Attitude Control      │
                                                    │ (общий для всех)    │
                                                    └────────┬────┘
                                                                             │
                                                                            ▼
                                                ┌────────────────────┐
                                                │ Rate Control → Motors → ESC │
                                                └────────────────────┘

📚 Архитектура: Position Control Code Overview

Детали по режимам

AltHold (только высота)

Throttle Stick → Deadzone (THR_DZ) → 
  ├─ Center: target_alt = current_alt
  ├─ Up: target_climb_rate = +PILOT_SPD_UP
  └─ Down: target_climb_rate = -PILOT_SPD_DN

Alt Error → PSC_POSZ_P(1.0) → Target Climb Rate
Climb Rate Error → PSC_VELZ_P(5.0) → Target Accel
Accel Error → PSC_ACCZ_P(0.5)/I(0.1)/D(0) → Throttle Output

Ключевые параметры:

Параметр	Описание	Значение
`PSC_POSZ_P`	Позиция → скорость набора	`1.0`
`PSC_VELZ_P`	Скорость → ускорение	`5.0`
`PSC_ACCZ_P/I`	Ускорение → газ	`0.5 / 0.1`
`PILOT_SPD_UP/DN`	Макс. скорость набора/снижения	`2.5 м/с`
`THR_DZ`	Мёртвая зона газа	`100` (10%)

📖 AltHold Mode [[1]]

PosHold (позиция + прямой угол)

Stick Input → Direct Angle Command (до ATC_ANGLE_MAX)
Stick Release → Brake Angle (PHLD_BRK_ANGLE) → Stop

Горизонталь:
  stick_angle → physics → estimated_vel → 
  vel_error × PSC_VELXY_P/I/D → brake_accel → stop

Вертикаль: тот же каскад, что в AltHold

Ключевые параметры:

Параметр	Описание	Значение
`PHLD_BRK_ANGLE`	Макс. угол торможения	`30°`
`PHLD_BRK_RATE`	Скорость возврата к торможению	`8°/с`
`PSC_VELXY_P/I`	Скорость → ускорение	`0.2 / 0.02`
`ATC_ANGLE_MAX`	Предельный угол наклона	`30-45°`

📖 PosHold Mode

Loiter (позиция + скорость)

Stick Input → Target Velocity (до LOIT_SPEED_MS)
Stick Release → Square Root Brake → Smooth Stop

Горизонталь:
  pos_error × PSC_POSXY_P(0.5) → target_vel
  target_vel = constrain(target_vel, ±LOIT_SPEED_MS)
  vel_error × PSC_VELXY_P/I/D → target_accel
  target_accel → target_angle → Attitude Controller

Торможение:
  brake_accel = -sign(vel) × sqrt(2 × LOIT_BRK_ACC_M × distance)

Ключевые параметры:

Параметр	Описание	Значение
`LOIT_SPEED_MS`	Макс. горизонтальная скорость	`4.0-5.0 м/с`
`LOIT_BRK_ACC_M`	Ускорение торможения	`2.5 м/с²`
`PSC_POSXY_P`	Позиция → скорость	`0.5-0.7`
`PSC_VELXY_P/I`	Скорость → ускорение	`0.2 / 0.02-0.04`
`LOIT_BRK_DELAY`	Задержка начала торможения	`0.0 с`

📖 Loiter Mode

Чек-лист настройки (РФ, без RTK, Quectel LC29H)

GPS & EKF

GPS_GNSS_MODE = 65          # L1+L5, все созвездия
GPS_AUTO_CONFIG = 1
EKF3_POSXY_GATE = 3.0       # Или 5.0 при шуме
EKF3_VELXY_GATE = 5.0
EKF3_YAW_GATE = 0.5

Общие для всех режимов

# Высота
PSC_POSZ_P = 1.0
PSC_VELZ_P = 5.0
PSC_ACCZ_P = 0.5
PSC_ACCZ_I = 0.1

# Горизонталь (для Loiter/PosHold)
PSC_POSXY_P = 0.5-0.7       # Не выше 0.8!
PSC_VELXY_P = 0.2
PSC_VELXY_I = 0.03          # Компенсация ветра
PSC_VELXY_D = 0.0

# Ограничения
LOIT_SPEED_MS = 4.0
LOIT_BRK_ACC_M = 2.5
ATC_ANGLE_MAX = 30-45°

PosHold специфика

PHLD_BRK_ANGLE = 30
PHLD_BRK_RATE = 8

Диагностика

# Проверка в Mission Planner:
# 1. FFT IMU: вибрации < 15 m/s²
# 2. GPS HDOP: ≤ 1.2 в холде
# 3. Компас: выполнить compassmot
# 4. Логи NTUN: DesVelX/Y ≈ VelX/Y

🔍 Быстрая диагностика проблем

Симптом	Вероятная причина	Решение
«Туалетное кольцо»	Магнитные помехи, ошибка компаса	`compassmot`, калибровка, экранирование
Дрейф в холде	`PSC_VELXY_I` низкий, ветер	Увеличить до `0.03-0.04`
Осцилляции позиции	`PSC_POSXY_P` > 0.8, вибрации	Снизить до `0.5-0.6`, проверить крепление
Рывок при входе в режим	Ошибка компаса >90°, EKF reset	Проверить ориентацию, `EKF3_YAW_GATE`
Медленный отклик	`PSC_VELXY_P` низкий, `LOIT_BRK_ACC_M` мал	Увеличить `PSC_VELXY_P` до `0.25`, `LOIT_BRK_ACC_M` до `3.0`

Визуализация отклика (пример: сдвиг стика вправо)

Время    Событие                               Позиция   Скорость   Угол
─────────────────────────────────────────────────────────────────
0мс      Стик Roll → 30% вправо                  0м        0 м/с     0°
10мс     target_vel = 1.5 м/с                          0м        0 м/с     0°
30мс     target_angle = 1.75°                      0.02м     0.1 м/с   0.5°
100мс    Дрон наклонился, разгон          0.3м      0.8 м/с   1.5°
2000мс   Стик отпущен                              3.0м      1.5 м/с   1.75°
2010мс   target_vel = 0, brake_angle        3.0м      1.5 м/с   -5°
2100мс   Торможение                                3.2м      0.5 м/с   -15°
2300мс   Фиксация позиции                    3.3м      0 м/с     0°

🔗 Источники

ℹ️ Для вашего случая: с модулем Quectel LC29H (L1+L5) и без RTK — оставьте PSC_POSXY_P = 0.5-0.6 для стабильного холда в условиях лесов/застройки Европейской РФ. Увеличьте PSC_VELXY_I до 0.03-0.04 для компенсации ветра. Проверьте GPS_GNSS_MODE = 65 для использования всех доступных созвездий.