Quaternion

Module ID: 9 (MODULE_QUATERNION) Algorytm: Madgwick filter (software)

Moduł fuzji orientacji. Oblicza kwaternion orientacji na podstawie danych z AccGyro i opcjonalnie Magnetometer.

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	`9`
Flags	`0x00`

Payload

Każda próbka składa się z 9 bajtów: 4 komponenty kwaternionu (po 2B) + 1 bajt statusu fuzji:

Quaternion sample (9B)
W	X	Y	Z	Status
2B LE	2B LE	2B LE	2B LE	1B
byte 0–1	byte 2–3	byte 4–5	byte 6–7	byte 8

Offset	Rozmiar	Pole	Format
0	2B	`w`	`int16_t LE` (Q15)
2	2B	`x`	`int16_t LE` (Q15)
4	2B	`y`	`int16_t LE` (Q15)
6	2B	`z`	`int16_t LE` (Q15)
8	1B	`status`	bitmaska (patrz niżej)

Bajt statusu fuzji

Bit	Maska	Nazwa	Opis
0	`0x01`	`MARG`	Filtr działa w trybie MARG (mag+acc+gyro); gdy `0` — tryb IMU (acc+gyro)
1	`0x02`	`MAG_ENABLED`	Magnetometr dostarcza próbki (próbka <2 s temu)
2	`0x04`	`MAG_CALIBRATED`	Skalibrowane próbki są odbierane
3	`0x08`	`MAG_VALID`	Norma pola magnetycznego w zakresie 40–60 µT
4	`0x10`	`MAG_FRESH`	Ostatnia skalibrowana próbka ma <30 ms

Diagnostyka gdy MARG=0

`MAG_ENABLED`	`MAG_CALIBRATED`	`MAG_VALID`	`MAG_FRESH`	Przyczyna
0	—	—	—	Magnetometr wyłączony
1	0	—	—	Brak kalibracji magnetometru
1	1	0	—	Zakłócenie pola magnetycznego (norma poza 40–60 µT)
1	1	1	0	Dane magnetometru nieświeże (>30 ms)

Format Q15

Wartości kwaternionu są w formacie Q1.15 (fixed-point):

Zakres: -1.0 do +0.999969
Konwersja: value = raw / 32768

Yaw (heading) z kwaternionu

Kąt yaw (obrót wokół osi Z, względem północy gdy MARG=1):

yaw = atan2(2·(w·z + x·y), 1 − 2·(y² + z²))

Wynik w radianach; zamień na stopnie: yaw_deg = yaw_rad × (180 / π).

Note

Yaw jest spójny z północą magnetyczną tylko gdy MARG=1. Gdy filtr działa w trybie IMU, yaw dryfuje w czasie.

Parsowanie (TypeScript)

interface QuaternionSample {
  w: number;
  x: number;
  y: number;
  z: number;
  status: number;
}

const FUSION_STATUS_MARG           = 0x01;
const FUSION_STATUS_MAG_ENABLED    = 0x02;
const FUSION_STATUS_MAG_CALIBRATED = 0x04;
const FUSION_STATUS_MAG_VALID      = 0x08;
const FUSION_STATUS_MAG_FRESH      = 0x10;

function parseQuaternionSample(view: DataView, offset: number): QuaternionSample {
  const toFloat = (raw: number) => raw / 32768;

  return {
    w:      toFloat(view.getInt16(offset + 0, true)),
    x:      toFloat(view.getInt16(offset + 2, true)),
    y:      toFloat(view.getInt16(offset + 4, true)),
    z:      toFloat(view.getInt16(offset + 6, true)),
    status: view.getUint8(offset + 8),
  };
}

function getYawDeg(q: QuaternionSample): number {
  const yawRad = Math.atan2(2 * (q.w * q.z + q.x * q.y),
                            1 - 2 * (q.y * q.y + q.z * q.z));
  return yawRad * (180 / Math.PI);
}

Control

Universal keys

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

Moduł Quaternion nie posiada kluczy moduł-specyficznych.

Wymagania

Aby fuzja działała prawidłowo, moduł AccGyro musi być włączony. Jeśli Magnetometer nie jest włączony, fuzja działa, ale nie uwzględnia danych magnetometru (brak korekcji heading).