# 🧭 ROADMAP: от чтения GPS к автономному роверу

> Этот документ — не готовый компонент, а план развития проекта. GPS-часть примера кода (шаг 6) уже реальна — `GpsNeo::getPosition()` действительно парсит координаты (см. `include/GpsNeo.h`); компас и управление моторами остаются концептом — для реальной сборки потребуется адаптация под твоё железо.

Управление автономным ровером по координатам — классическая задача робототехники. Она разбивается на две составляющие: **геометрию** (куда ехать?) и **алгоритм управления** (как крутить моторы, чтобы туда попасть).

## 1. Зачем ровер GPS + компас, а не только GPS

GPS обновляется довольно редко — обычно 1 раз в секунду (1 Гц), а на низкой скорости или в момент остановки курс движения (`course over ground`) начинает "дрожать": по двум близким точкам направление посчитать надёжно нельзя. Если управлять ровером только по спутникам, он будет постоянно вилять и проскакивать точки.

Поэтому нужен **компас** (магнитометр/гироскоп) — датчик, который знает, куда ровер повёрнут прямо сейчас, с частотой 50–100 Гц, а не 1 Гц. Комбинация "редкие, но абсолютные координаты GPS" + "частый, но дрейфующий поворот компаса" и даёт стабильное управление.

## 2. Математика навигации: две главные формулы

Микроконтроллер ровера каждую секунду должен знать два параметра:

1. **Distance** — сколько метров осталось до целевой точки.
2. **Bearing** (Target Heading) — на какой угол относительно севера нужно повернуть ровер, чтобы он смотрел прямо на цель.

### 2.1 Расстояние — формула гаверсинусов

Земля круглая, поэтому обычная теорема Пифагора на больших расстояниях врёт. Для робототехники используют формулу гаверсинусов (Haversine formula).

- Вход: `(Lat1, Lon1)` — где ровер сейчас, `(Lat2, Lon2)` — куда ему надо.
- Выход: расстояние в метрах.

### 2.2 Целевой курс — Bearing

Формула высчитывает угол от 0° (Север) до 359° по часовой стрелке — куда должен смотреть ровер, чтобы двигаться прямо на цель.

> Обе формулы — в коде на шаге 6 (`getDistance()`, `getBearing()`).

## 3. Компас: акселерометр и гироскоп (MPU6050)

### 3.1 Что измеряет MPU6050

- **Акселерометр:** линейное ускорение по 3 осям (`ax, ay, az`), в единицах `g` (1 g ≈ 9.81 м/с²).
- **Гироскоп:** угловая скорость по 3 осям (`gx, gy, gz`), в °/с.

### 3.2 Преобразование регистров в физические величины

- Диапазон акселерометра ±2g: `raw / 16384.0` → g.
- Диапазон гироскопа ±250°/с: `raw / 131.0` → °/с.

### 3.3 Расчёт углов по акселерометру

$$ \text{roll}_{acc} = \operatorname{atan2}(a_y, a_z) \cdot \frac{180}{\pi} $$

$$ \text{pitch}_{acc} = \operatorname{atan2}(-a_x, \sqrt{a_y^2 + a_z^2}) \cdot \frac{180}{\pi} $$

### 3.4 Фильтрация: комплементарный фильтр

Акселерометр даёт точный угол в статике, но "шумит" при вибрации. Гироскоп даёт точный мгновенный поворот, но дрейфует со временем (ошибка накапливается). Комплементарный фильтр берёт от каждого источника только то, в чём он хорош:

$$ \theta = \alpha (\theta + \omega_{gyro} \Delta t) + (1-\alpha) \theta_{acc} $$

где $\alpha \in [0,1]$ (обычно ≈ 0.98 — почти полностью доверяем гироскопу, но понемногу "подтягиваем" к акселерометру, чтобы скомпенсировать дрейф).

Именно эта комбинация (GPS для абсолютных координат + комплементарный фильтр компаса для стабильного мгновенного курса) даёт ровный ход автономного ровера.

## 4. Архитектура железа ровера

Для стабильного движения нужна связка из трёх компонентов:

1. **GNSS-модуль (наш NEO-8M)** — даёт абсолютные координаты. Обновляется медленно (1–5 Гц).
2. **Цифровой компас** (магнитометр, например QMC5883L / LSM303, или связка акселерометр+гироскоп MPU6050 из раздела 3) — даёт Current Heading, куда ровер повёрнут прямо сейчас. Обновляется быстро (50–100 Гц). Без него ровер не поймёт, в какую сторону он развёрнут, пока не проедет несколько метров.
3. **Приводы** (драйвер моторов) — выполняет команды контроллера.

## 5. Алгоритм управления в loop()

Логика автономного движения работает по циклу обратной связи:

```
[Читаем GPS] ──> Расчёт целевого курса (Bearing)
                       │
[Читаем компас] ──> Получение текущего курса (Heading)
                       │
                       ▼
            Расчёт ошибки: Error = Bearing - Heading
                       │
                       ▼
            Коррекция моторов (P-регулятор / PID)
```

1. **Вычисляем ошибку курса.** Например, целевой курс (Bearing) = 90° (Восток), компас показывает 70°. Ошибка = +20° (нужно довернуть направо).
   Важно: ошибку нужно нормализовать в диапазон от −180° до +180°, чтобы ровер не разворачивался на 270° направо, если можно повернуться на 90° налево.
2. **Применяем пропорциональный регулятор (P-регулятор).** Скорость вращения должна быть пропорциональна ошибке: большая ошибка (90°) — крутимся на месте на максимальной скорости, маленькая (5°) — слегка подруливаем на ходу.
3. **Проверяем финиш.** Если расстояние до цели стало меньше, например, 1.5–2 метров — точка достигнута, останавливаемся или переключаемся на следующую точку маршрута.
   Добиться точности "0 метров" на обычном GPS нельзя — из-за погрешности в 2–3 метра ровер будет бесконечно кружить вокруг точки.

## 6. Пример кода (концепт для ESP32)

```cpp
#include <Arduino.h>
#include <Wire.h>

// Целевая точка (куда едем)
const double TARGET_LAT = 55.630340;
const double TARGET_LON = 38.068356;

// Допустимый радиус прибытия (в метрах)
const float ARRIVAL_RADIUS = 2.0;

double toRadians(double degree) { return degree * PI / 180.0; }
double toDegrees(double radian) { return radian * 180.0 / PI; }

// 1. Расчёт расстояния (Haversine)
double getDistance(double lat1, double lon1, double lat2, double lon2) {
    double R = 6371000.0; // Радиус Земли в метрах
    double dLat = toRadians(lat2 - lat1);
    double dLon = toRadians(lon2 - lon1);
    double a = sin(dLat / 2) * sin(dLat / 2) +
               cos(toRadians(lat1)) * cos(toRadians(lat2)) *
               sin(dLon / 2) * sin(dLon / 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));
    return R * c;
}

// 2. Расчёт целевого угла (Bearing)
double getBearing(double lat1, double lon1, double lat2, double lon2) {
    double lat1Rad = toRadians(lat1);
    double lat2Rad = toRadians(lat2);
    double dLon = toRadians(lon2 - lon1);

    double y = sin(dLon) * cos(lat2Rad);
    double x = cos(lat1Rad) * sin(lat2Rad) - sin(lat1Rad) * cos(lat2Rad) * cos(dLon);

    double bearing = toDegrees(atan2(y, x));
    if (bearing < 0) bearing += 360.0;
    return bearing;
}

// Имитация чтения компаса (в реальности — опрос датчика, см. раздел 3)
float getCompassHeading() {
    return 45.0; // Допустим, ровер сейчас смотрит на Северо-Восток
}

void driveMotors(int forwardSpeed, int turnSpeed) {
    // Логика управления драйвером моторов (например, L298N или BTS7960)
    // forwardSpeed - езда вперёд, turnSpeed - разность скоростей бортов для поворота
}

// GpsNeo gps(Serial2); — уже реализовано и парсит координаты по-настоящему
// (не концепт): gps.getPosition() отдаёт latitude/longitude/speedKmh/courseDeg,
// см. include/GpsNeo.h. Ниже — как использовать это в цикле ровера.
extern GpsNeo gps;

void loop() {
    const Position &pos = gps.getPosition();
    if (!pos.valid) return; // ждём валидный fix, см. GUIDE.md, раздел 5

    double currentLat = pos.latitude;
    double currentLon = pos.longitude;

    double distance = getDistance(currentLat, currentLon, TARGET_LAT, TARGET_LON);

    if (distance < ARRIVAL_RADIUS) {
        Serial.println("Точка достигнута! Стоп.");
        driveMotors(0, 0); // Тормозим
        return;
    }

    double targetHeading = getBearing(currentLat, currentLon, TARGET_LAT, TARGET_LON);
    double currentHeading = getCompassHeading();

    // Считаем ошибку курса
    double headingError = targetHeading - currentHeading;

    // Нормализация угла в диапазон [-180, 180]
    if (headingError > 180)  headingError -= 360;
    if (headingError < -180) headingError += 360;

    // Пропорциональный коэффициент подруливания (настраивается экспериментально)
    float Kp = 2.5;
    int turnSpeed = headingError * Kp;

    // Ограничиваем скорость поворота
    turnSpeed = constrain(turnSpeed, -100, 100);

    // Едем вперёд и подруливаем
    int forwardSpeed = 150; // Базовая скорость хода вперёд
    driveMotors(forwardSpeed, turnSpeed);

    delay(100);
}
```

## 7. Проблемы, с которыми ты столкнёшься, и как их решать

1. **Помехи на компас от моторов.** Силовые провода и сами моторы создают сильные магнитные поля. Компас нужно выносить на высокой немагнитной мачте (как на квадрокоптерах), как можно дальше от аккумулятора и двигателей.
2. **GPS "скачет" на месте.** Когда ровер стоит, координаты немного "плывут". Из-за этого вычисленный `targetHeading` начнёт хаотично прыгать. Решение: если скорость ровера равна 0 или расстояние до цели слишком мало — запретить обновление целевого курса.
3. **Инерция.** Ровер тяжёлый и поворачивает не сразу. Если он проскакивает нужный угол и начинает вилять (осциллировать) — нужно уменьшать коэффициент `Kp` или переходить на полноценный PID-регулятор (добавить дифференциальную составляющую).