AccGyro

Module ID: 2 (MODULE_ACC_GYRO) Sensor: LSM6DSV32X (ST Microelectronics)

Moduł akcelerometru i żyroskopu.

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	2
Flags	0x00

Payload

Każda próbka składa się z 18 bajtów (6 osi × 3B little-endian signed int24):

Accelerometer			Gyroscope
Acc X	Acc Y	Acc Z	Gyro X	Gyro Y	Gyro Z
3B LE	3B LE	3B LE	3B LE	3B LE	3B LE
byte 0–2	byte 3–5	byte 6–8	byte 9–11	byte 12–14	byte 15–17

Offset	Rozmiar	Pole	Format	Jednostka	Przelicznik
0	3B	acc_x	int24 LE (signed)	deci-milli-g	÷ 10000 → g
3	3B	acc_y	int24 LE (signed)	deci-milli-g	÷ 10000 → g
6	3B	acc_z	int24 LE (signed)	deci-milli-g	÷ 10000 → g
9	3B	gyro_x	int24 LE (signed)	milli-°/s	÷ 1000 → °/s
12	3B	gyro_y	int24 LE (signed)	milli-°/s	÷ 1000 → °/s
15	3B	gyro_z	int24 LE (signed)	milli-°/s	÷ 1000 → °/s

Rozdzielczość

• Akcelerometr: 0.1 mg (1 LSB = 0.0001 g)
• Żyroskop: 1 milli-°/s (1 LSB = 0.001 °/s)

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

Parametry domyślne

Parametr	Wartość domyślna
Zakres akcelerometru	±32 g
Zakres żyroskopu	±1000 dps
ODR	240 Hz

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 parseAccGyroSample(view: DataView, offset: number) {
  const accX  = readInt24LE(view, offset + 0)  / 10000; // g
  const accY  = readInt24LE(view, offset + 3)  / 10000; // g
  const accZ  = readInt24LE(view, offset + 6)  / 10000; // g
  const gyroX = readInt24LE(view, offset + 9)  / 1000;  // °/s
  const gyroY = readInt24LE(view, offset + 12) / 1000;  // °/s
  const gyroZ = readInt24LE(view, offset + 15) / 1000;  // °/s
  return { accX, accY, accZ, gyroX, gyroY, gyroZ };
}

Control

Klucze konfiguracyjne (universalne)

Key	Wartość	Typ	Opis
KEY_CFG_ODR	0x10	SET/GET	Częstotliwość próbkowania [Hz] (uint16_t LE)
KEY_CFG_MEASURE_RANGE_1	0x11	SET/GET	Zakres akcelerometru [g] (uint16_t LE)
KEY_CFG_MEASURE_RANGE_2	0x12	SET/GET	Zakres żyroskopu [dps] (uint16_t LE)
KEY_CFG_RAW_MODE	0x15	SET/GET	Tryb surowych danych żyroskopu (uint8_t): 0 = skorygowane, 1 = raw. SET 0 bez punktów kalibracyjnych → NOT_READY.

Klucze moduł-specyficzne (0x80+)

Key	Wartość	Typ	Opis
KEY_AG_GET_TEMPERATURE	0x80	GET	Bieżąca temperatura chipa IMU — int16_t LE, jednostka 0.1°C (decidegC). Zawsze dostępna, niezależnie od stanu kalibracji.
KEY_AG_DELETE_CALIB_POINT	0x81	SET	Usuń punkt kalibracyjny o podanej temperaturze. value[0–1] = T (int16_t LE, decidegC). Zapisuje NVS. Brak punktu → INVALID_VALUE.

Kalibracja biasu żyroskopu

Moduł AccGyro obsługuje wielopunktową kalibrację biasu żyroskopu z interpolacją temperaturową. Każdy punkt kalibracyjny przechowuje bias mierzony w konkretnej temperaturze. Firmware interpoluje liniowo między punktami i ekstrapoluje poza zakres.

Zalecenia: - Mierz w różnych temperaturach (co ~5–10°C różnicy) - Minimalna odległość między punktami: 5°C (weryfikacja po stronie klienta) - Maksymalnie 10 punktów - Urządzenie powinno leżeć nieruchomo podczas pomiaru (~20 s)

Sekwencja dodania punktu kalibracyjnego

flowchart LR
    A[CALIB_START] --> B[polling CALIB_STATUS\nco 500 ms]
    B --> C{progress = 100%?}
    C -->|nie| B
    C -->|tak| D[CALIB_COMMIT]
    A --> E[CALIB_ABORT]

Klucze kalibracyjne

Key	Wartość	Typ	Opis
KEY_CALIB_START	0x20	SET	Rozpocznij pomiar (brak value). BUSY jeśli trwa. Max 10 punktów → INVALID_VALUE.
KEY_CALIB_ABORT	0x21	SET	Przerwij aktywny pomiar.
KEY_CALIB_COMMIT	0x23	SET	Zatwierdź punkt — dodaj do posortowanej listy i zapisz do NVS. NOT_READY jeśli pomiar nieaktywny lub za mało próbek.
KEY_CALIB_GET_BLOB	0x24	GET	Odczytaj blob kalibracyjny 164B (patrz niżej).
KEY_CALIB_SET_BLOB	0x25	SET	Importuj blob kalibracyjny 164B. Auto-save do NVS.
KEY_CALIB_STATUS	0x26	GET	Status kalibracji (patrz niżej).
KEY_AG_DELETE_CALIB_POINT	0x81	SET	Usuń punkt po temperaturze.

CALIB_STATUS — format odpowiedzi

Offset	Rozmiar	Pole	Opis
0	1B	count	Liczba zatwierdzonych punktów (0–10)
1	1B	byte1	Jeśli pomiar aktywny: postęp 0–100%. Jeśli nieaktywny: raw_mode (0/1).
2–3	2B	T_now	Bieżąca temperatura chipa (int16_t LE, decidegC = 0.1°C)
4–15	12B	avg_bias	Opcjonalne (tylko gdy pomiar aktywny i są próbki). Bieżąca średnia biasu [X, Y, Z] jako 3× int32_t LE [milli-°/s].

Blob kalibracyjny — format (164 bajtów)

Blob to serializacja struktury ag_calib_nvs_t:

Nagłówek (4B)		Punkty kalibracyjne — do 10 × 16B (160B)
count	reserved[3]	point[0]	point[1]	…	point[9]
1B	3B	16B	16B	…	16B

Każdy punkt kalibracyjny (ag_bias_point_t, 16B):

Offset w punkcie	Rozmiar	Pole	Format	Opis
0	2B	T	int16_t LE	Temperatura pomiaru [decidegC]
2	2B	_pad	—	Wyrównanie (ignorować)
4	4B	bias_x	int32_t LE	Bias osi X [milli-°/s]
8	4B	bias_y	int32_t LE	Bias osi Y [milli-°/s]
12	4B	bias_z	int32_t LE	Bias osi Z [milli-°/s]

Punkty posortowane rosnąco po T. Indeksy count do 9 zawierają zerowe dane — ignorować.

Parsowanie bloba (TypeScript)

interface GyroBiasPoint {
  tempDegC: number;
  biasX_dps: number;
  biasY_dps: number;
  biasZ_dps: number;
}

function parseGyroBiasBlob(data: DataView): GyroBiasPoint[] {
  const count = Math.min(data.getUint8(0), 10);
  const points: GyroBiasPoint[] = [];

  for (let i = 0; i < count; i++) {
    const base = 4 + i * 16;
    const T_decidegC = data.getInt16(base, true);
    const bx = data.getInt32(base + 4, true);
    const by = data.getInt32(base + 8, true);
    const bz = data.getInt32(base + 12, true);
    points.push({
      tempDegC: T_decidegC / 10,
      biasX_dps: bx / 1000,
      biasY_dps: by / 1000,
      biasZ_dps: bz / 1000,
    });
  }
  return points;
}

Odczyt temperatury chipa

TX: [0x01] [0x02] [0x02] [0x80]
      tid   ag    GET   GET_TEMPERATURE

RX: [0x01] [0x02] [0x02] [0x80] [0x01] [0xFA] [0x00]
      tid   ag    GET   GET_TEMPERATURE  OK   250 → 25.0°C

Usunięcie punktu kalibracyjnego

TX: [0x02] [0x02] [0x01] [0x81] [0xFA] [0x00]
      tid   ag    SET  DELETE_POINT   250 (25.0°C, int16 LE)

RX: [0x02] [0x02] [0x01] [0x81] [0x01]
      tid   ag    SET  DELETE_POINT   OK

Universal keys

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