Skip to content

Displacement

Module ID: 13 (MODULE_DISPLACEMENT)

Moduł estymacji przemieszczenia na podstawie akcelerometru. Używa trzyosiowego filtra Kalmana z algorytmem ZUPT (Zero Velocity Update) i konfigurowalnym soft constraint prędkości, aby minimalizować dryfowanie.

Algorytm

Każda próbka z acc/gyro jest przetwarzana przez filtr Kalmana (position, velocity, acc_bias). W stanach spoczynku ZUPT zeruje prędkość. Opcjonalny soft constraint (KEY_DISP_SOFT_R) aplikuje słabą korekcję prędkości do zera każdej próbce — zmniejsza dryfowanie przy wolnym ruchu kosztem tłumienia długich ruchów jednostajnych.

Data Frame

Header (12B)
Module ID Flags Frame Nr Sample Count Timestamp First Timestamp Last Δ
1B 1B 1B 1B 5B LE 3B LE
byte 0 byte 1 byte 2 byte 3 byte 4–8 byte 9–11
Pole Wartość
Module ID 13
Flags 0x00

Payload

Każda próbka składa się z 12 bajtów (3 osie × 4B int32 little-endian signed).

Displacement sample (12B)
Pos X Pos Y Pos Z
4B LE 4B LE 4B LE
byte 0–3 byte 4–7 byte 8–11
Offset Rozmiar Pole Format Jednostka Przelicznik
0 4B pos_x int32_t little-endian milimetry [mm] ÷ 1000 → m
4 4B pos_y int32_t little-endian milimetry [mm] ÷ 1000 → m
8 4B pos_z int32_t little-endian milimetry [mm] ÷ 1000 → m

Łączny rozmiar payload = sample_count × 12 bajtów.

Parsowanie (TypeScript)

function parseDisplacementSample(view: DataView, offset: number) {
  return {
    x_m: view.getInt32(offset + 0, true) / 1000,
    y_m: view.getInt32(offset + 4, true) / 1000,
    z_m: view.getInt32(offset + 8, true) / 1000,
  };
}

Control

Klucze moduł-specyficzne

Key Wartość Typ Opis
KEY_DISP_RESET 0x80 SET Reset stanu KF: pozycja → 0, prędkość → 0, bias acc → 0. Brak value. Nowe origin = bieżąca pozycja.
KEY_DISP_SOFT_R 0x81 SET/GET Siła korekcji soft constraint prędkości. uint16_t LE, jednostka 0.1 m²/s². 0 = wyłączone. Domyślnie 40 (= 4.0 m²/s²). Większe → słabsza korekcja (lepsze śledzenie ruchów). Mniejsze → silniejsza korekcja (mniej dryfu). Zapisywane do NVS.

Dobór KEY_DISP_SOFT_R

  • 0 — wyłączony soft constraint. Czyste całkowanie inercyjne. Szybki dryfowanie.
  • 10–20 — silna korekcja. Dobry spoczynek, tłumi długie ruchy jednostajne.
  • 40 (domyślnie) — kompromis. 4.0 m²/s² szumu pomiaru wirtualnego.
  • 80–100 — słaba korekcja. Lepsze śledzenie ruchów, więcej dryfu w spoczynku.

Universal keys

  • KEY_CTRL_INFO (0x01) — informacje o module
  • KEY_CTRL_ENABLE (0x02) — włącz/wyłącz moduł

Zależność od AccGyro

Moduł Displacement subskrybuje próbki z modułu AccGyro przez wewnętrzny bus. AccGyro musi być włączony, żeby Displacement otrzymywał dane.