Magnetometer
Module ID: 6 (MODULE_MAGNETOMETER)
Sensor: MMC5983MA (MEMSIC)
Moduł magnetometru. Mierzy pole magnetyczne w trzech osiach. Służy do wyznaczania azymutu (heading) i jest wykorzystywany przez moduł fuzji orientacji.
Tryby danych: raw vs calibrated
Magnetometr może wysyłać dane w dwóch formatach — raw (surowe) i calibrated (skalibrowane). Tryb wpływa na to, co trafia do ramek BLE Data Frame i co otrzymuje wewnętrzny moduł fusion.
Definicje
| Tryb | Opis |
|---|---|
| raw | Dane po odjęciu bridge offset, konwersji na nT i remapie osi. Bez korekcji hard-iron i soft-iron. |
| calibrated | Dane po pełnej kalibracji: bridge offset → nT → remap osi → hard-iron → soft-iron. |
Jednostka w obu trybach jest taka sama: nanoTesla (nT) w ramce BLE (÷ 1000 → µT).
Logika publikacji
Zachowanie zależy od stanu kalibracji (s_calibrated) i trybu raw (KEY_CFG_RAW_MODE):
| Stan | Tryb raw | Co trafia do BLE (Data Frame) | Co trafia do fusion |
|---|---|---|---|
| Brak kalibracji | — | 1× raw (flag 0) |
1× raw (flag 0) — fusion ignoruje dane bez flagi calibrated |
| Skalibrowany | 0 (domyślny) |
1× calibrated (flag 1) |
1× calibrated (flag 1) |
| Skalibrowany | 1 |
1× raw (flag 0) |
1× calibrated (flag 1, NO_AGGREGATE — nie trafia do BLE) |
Tryb raw z kalibracją
Gdy kalibracja istnieje i włączony jest tryb raw, firmware publikuje dwie próbki
na zbus: surową (dla BLE/agregatora) i skalibrowaną (tylko dla fusion, z flagą
NO_AGGREGATE — nie jest pakowana do Data Frame BLE).
Data Frame
Header
| 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 | 6 |
| Flags | Patrz sekcja Flagi |
Flagi
| Bit | Pole | Opis |
|---|---|---|
| 7 | FLAG_ERROR |
Ramka błędu (standardowa) |
| 1–6 | Reserved | — |
| 0 | CALIBRATED |
0 = dane raw (bez hard/soft iron), 1 = dane po pełnej kalibracji |
Payload
Każda próbka składa się z 9 bajtów (3 osie × 3B little-endian signed int24). Format próbki jest identyczny niezależnie od trybu raw/calibrated — różni je flaga w headerze.
| Magnetometer sample (9B) | ||
| Mag X | Mag Y | Mag Z |
| 3B LE | 3B LE | 3B LE |
| byte 0–2 | byte 3–5 | byte 6–8 |
| Offset | Rozmiar | Pole | Format | Jednostka | Przelicznik |
|---|---|---|---|---|---|
| 0 | 3B | mag_x |
int24 little-endian (signed) |
nanoTesla (nT) | ÷ 1000 → µT |
| 3 | 3B | mag_y |
int24 little-endian (signed) |
nanoTesla (nT) | ÷ 1000 → µT |
| 6 | 3B | mag_z |
int24 little-endian (signed) |
nanoTesla (nT) | ÷ 1000 → µT |
Rozdzielczość
1 nT (1 LSB = 0.001 µT). Typowe ziemskie pole magnetyczne: 40 000–60 000 nT.
Łączny rozmiar payload = sample_count × 9 bajtów.
Parsowanie (TypeScript)
function readInt24LE(view: DataView, offset: number): number {
const lo = view.getUint8(offset);
const mi = view.getUint8(offset + 1);
const hi = view.getInt8(offset + 2); // sign-extended
return lo | (mi << 8) | (hi << 16);
}
function parseMagSample(view: DataView, offset: number, flags: number) {
const isCalibrated = (flags & 0x01) !== 0;
const x_uT = readInt24LE(view, offset + 0) / 1000; // nT → µT
const y_uT = readInt24LE(view, offset + 3) / 1000;
const z_uT = readInt24LE(view, offset + 6) / 1000;
return { x_uT, y_uT, z_uT, isCalibrated };
}
Control
Klucze konfiguracyjne (uniwersalne)
| Key | Wartość | Typ | Opis |
|---|---|---|---|
KEY_CFG_ODR |
0x10 |
SET/GET | Częstotliwość próbkowania [Hz] (uint16_t little-endian). Restart CMM jeśli moduł aktywny. |
KEY_CFG_FILTER |
0x14 |
SET/GET | Bandwidth [Hz] (uint16_t little-endian). Restart CMM jeśli moduł aktywny. |
KEY_CFG_RAW_MODE |
0x15 |
SET/GET | Tryb surowych danych (uint8_t): 0 = calibrated (domyślny), 1 = raw. SET 0 bez kalibracji → NOT_READY. |
Klucze kalibracji
Magnetometr nie obsługuje interaktywnej sekwencji kalibracji (CALIB_START / CAPTURE / COMMIT / ABORT).
Kalibracja wykonywana jest po stronie klienta (np. aplikacja mobilna), a wynik importowany przez CALIB_SET_BLOB.
| Key | Wartość | Typ | Opis |
|---|---|---|---|
KEY_CALIB_GET_BLOB |
0x24 |
GET | Odczytaj aktualny blob kalibracyjny (42B) |
KEY_CALIB_SET_BLOB |
0x25 |
SET | Zapisz blob kalibracyjny (import). Auto-save do NVS. Wyłącza tryb raw. |
KEY_CALIB_STATUS |
0x26 |
GET | Status kalibracji — patrz tabela poniżej |
CALIB_STATUS — wartości odpowiedzi
value[0] |
Znaczenie |
|---|---|
0 |
Brak kalibracji (domyślne wartości) |
1 |
Skalibrowany, tryb calibrated |
2 |
Skalibrowany, tryb raw |
Blob kalibracyjny (42B)
Blob przesyłany przez KEY_CALIB_GET_BLOB / KEY_CALIB_SET_BLOB ma stały rozmiar 42 bajtów
i zawiera parametry hard-iron oraz soft-iron w ramie logicznej (po remapie osi).
| Hard-iron (6B) | Soft-iron matrix 3×3 (36B) | ||||||||||
| HI X | HI Y | HI Z | SI [0][0] | SI [0][1] | SI [0][2] | SI [1][0] | SI [1][1] | SI [1][2] | SI [2][0] | SI [2][1] | SI [2][2] |
| 2B | 2B | 2B | 4B | 4B | 4B | 4B | 4B | 4B | 4B | 4B | 4B |
| 0–1 | 2–3 | 4–5 | 6–9 | 10–13 | 14–17 | 18–21 | 22–25 | 26–29 | 30–33 | 34–37 | 38–41 |
| Offset | Rozmiar | Pole | Format | Jednostka |
|---|---|---|---|---|
| 0 | 2B | hard_iron_x |
int16_t little-endian |
deci-µT (1 LSB = 0.1 µT) |
| 2 | 2B | hard_iron_y |
int16_t little-endian |
deci-µT |
| 4 | 2B | hard_iron_z |
int16_t little-endian |
deci-µT |
| 6 | 4B | soft_iron[0][0] |
float32 little-endian |
bezwymiarowa |
| 10 | 4B | soft_iron[0][1] |
float32 little-endian |
bezwymiarowa |
| 14 | 4B | soft_iron[0][2] |
float32 little-endian |
bezwymiarowa |
| 18 | 4B | soft_iron[1][0] |
float32 little-endian |
bezwymiarowa |
| 22 | 4B | soft_iron[1][1] |
float32 little-endian |
bezwymiarowa |
| 26 | 4B | soft_iron[1][2] |
float32 little-endian |
bezwymiarowa |
| 30 | 4B | soft_iron[2][0] |
float32 little-endian |
bezwymiarowa |
| 34 | 4B | soft_iron[2][1] |
float32 little-endian |
bezwymiarowa |
| 38 | 4B | soft_iron[2][2] |
float32 little-endian |
bezwymiarowa |
Hard-iron w blobie vs w ramce Data
Hard-iron w blobie jest w deci-µT (int16_t). Dane w ramce Data są w nanoTesla (int24).
To dwie różne reprezentacje tej samej jednostki fizycznej — nie mylić.
Domyślne wartości
Bez kalibracji: hard_iron = {0, 0, 0}, soft_iron = macierz identyczności
(przekątna 1.0, reszta 0.0).
Bridge offset
Bridge offset (kompensacja mostka SET/RESET sensora MMC5983MA) nie jest częścią bloba.
Jest mierzony automatycznie przy magnetometer_init() i przechowywany wewnętrznie.
Parsowanie bloba (TypeScript)
function parseMagCalibBlob(data: DataView) {
const hard_iron_x_uT = data.getInt16(0, true) / 10; // deci-µT → µT
const hard_iron_y_uT = data.getInt16(2, true) / 10;
const hard_iron_z_uT = data.getInt16(4, true) / 10;
const soft_iron: number[][] = [];
for (let row = 0; row < 3; row++) {
soft_iron[row] = [];
for (let col = 0; col < 3; col++) {
soft_iron[row][col] = data.getFloat32(6 + (row * 3 + col) * 4, true);
}
}
return { hard_iron_x_uT, hard_iron_y_uT, hard_iron_z_uT, soft_iron };
}
Budowanie bloba (TypeScript)
function buildMagCalibBlob(
hard_iron_uT: [number, number, number],
soft_iron: number[][]
): ArrayBuffer {
const buf = new ArrayBuffer(42);
const view = new DataView(buf);
// hard-iron: µT → deci-µT (int16)
view.setInt16(0, Math.round(hard_iron_uT[0] * 10), true);
view.setInt16(2, Math.round(hard_iron_uT[1] * 10), true);
view.setInt16(4, Math.round(hard_iron_uT[2] * 10), true);
// soft-iron: float32, row-major
for (let row = 0; row < 3; row++) {
for (let col = 0; col < 3; col++) {
view.setFloat32(6 + (row * 3 + col) * 4, soft_iron[row][col], true);
}
}
return buf;
}
Klucze moduł-specyficzne
| Key | Wartość | Typ | Opis |
|---|---|---|---|
KEY_MAG_NORM_TOL |
0x80 |
SET/GET | Tolerancja normy pola (uint16_t LE, jednostka nT, domyślnie 10000 = 10 µT). Definiuje szerokość okna akceptacji: [norm_ref − tol, norm_ref + tol]. SET zapisuje do NVS. |
KEY_MAG_NORM_REF |
0x81 |
GET | Bieżąca norma referencyjna (uint32_t LE, nT). Domyślnie 50000 (50 µT). Aktualizowana automatycznie po kalibracji (KEY_CALIB_SET_BLOB) z pierwszych 50 skalibrowanych próbek. |
Przykłady
Włączenie trybu raw
TX: [0x01] [0x06] [0x01] [0x15] [0x01]
tid mag SET RAW_MODE 1
RX: [0x01] [0x06] [0x01] [0x15] [0x01]
tid mag SET RAW_MODE OK
Jeśli brak kalibracji, próba wyłączenia raw mode (value=0) zwróci NOT_READY (0x05).
Odczyt statusu kalibracji
TX: [0x02] [0x06] [0x02] [0x26]
tid mag GET CALIB_STATUS
RX: [0x02] [0x06] [0x02] [0x26] [0x01] [0x01]
tid mag GET CALIB_STATUS OK calibrated
Import kalibracji (SET blob)
TX: [0x03] [0x06] [0x01] [0x25] [42 bajty danych kalibracyjnych]
tid mag SET CALIB_SET_BLOB
RX: [0x03] [0x06] [0x01] [0x25] [0x01]
tid mag SET CALIB_SET_BLOB OK
Po udanym imporcie: kalibracja zostaje zastosowana, zapisana do NVS, tryb raw jest wyłączany.
Jeśli blob ma nieprawidłowy rozmiar (< 42B) → INVALID_VALUE (0x03).
Odczyt kalibracji (GET blob)
TX: [0x04] [0x06] [0x02] [0x24]
tid mag GET CALIB_GET_BLOB
RX: [0x04] [0x06] [0x02] [0x24] [0x01] [42 bajty bloba kalibracyjnego]
tid mag GET CALIB_GET_BLOB OK hard_iron(6B) + soft_iron(36B)
Zmiana ODR
TX: [0x05] [0x06] [0x01] [0x10] [0x64] [0x00]
tid mag SET ODR 100 Hz (uint16 LE)
RX: [0x05] [0x06] [0x01] [0x10] [0x01]
tid mag SET ODR OK
Nieobsługiwana wartość → INVALID_VALUE (0x03). Jeśli moduł jest aktywny, CMM jest restartowany z nową częstotliwością.
Universal keys
KEY_CTRL_INFO(0x01) — informacje o module (model, obsługiwane ODR i bandwidth w TLV)KEY_CTRL_ENABLE(0x02) — włącz/wyłącz moduł (uint8_t:1= włącz,0= wyłącz)
Fuzja
Aby fuzja orientacji (moduł Quaternion) korzystała z magnetometru, magnetometr musi być włączony i skalibrowany. Bez magnetometru fuzja nadal działa, ale opiera się wyłącznie na acc/gyro.