Přesná 3D orientace z TriSense Pro

Obsah návodu6 kapitol

1Role jednotlivých senzorů
2Zapojení pro fúzi
3Metoda 1: SimpleTriFusion
4Metoda 2: AdvancedTriFusion
5Srovnání obou metod
6Tipy a řešení problémů

1Role jednotlivých senzorů

Kompletní prostorová orientace — náklon (roll), stoupání (pitch) a kurz (yaw) — vyžaduje data ze všech tří čipů na desce: gyroskopu a akcelerometru z ICM42688-P a magnetometru AK09918C. Žádný z nich to nezvládne sám, každý má jinou slabinu.

Gyroskop (ICM42688-P): měří úhlovou rychlost. Skvěle sleduje rychlé, dynamické pohyby, ale v čase se v datech hromadí chyba — drift.
Akcelerometr (ICM42688-P): ukazuje směr gravitace, takže dobře určí roll a pitch v klidu. Během pohybu je ale nespolehlivý — měří i vlastní zrychlení, nejen gravitaci.
Magnetometr (AK09918C): měří směr magnetického pole Země a slouží k určení kurzu. Nedriftuje, ale je pomalý a citlivý na okolní rušení.

Knihovna TriSense nabízí dvě implementace fúze, které se liší přesností i náročností: SimpleTriFusion a AdvancedTriFusion.

2Zapojení pro fúzi

Fúze používá hybridní režim: magnetometr a barometr běží po I²C, IMU po rychlejším SPI. Potřebujete proto sedm propojovacích vodičů.

VIN5Vlogika Arduina Uno

GNDGNDspolečná zem

SDAA4I²C data

SCLA5I²C hodiny

SID11MOSI

SOD12MISO

SCKD13SPI hodiny

CSD10lze změnit

I2C zapojení k Raspberry Pi Pico ze strany spodní části desky — TriSense Pro zapojený k Raspberry Pi Pico.

3Metoda 1: SimpleTriFusion

Nejjednodušší přístup, který spoléhá na to, že gyroskop je krátkodobě dostatečně přesný.

Počáteční stav: v klidu se zavolá fusion.initOrientation(), které spočítá výchozí orientaci — roll a pitch z akcelerometru, yaw z magnetometru.
Sledování: orientace se dál počítá pouze integrací gyroskopu s časovým krokem Δt: nový úhel = starý úhel + (úhlová rychlost × Δt). Ostatní senzory se už nepoužívají.

SimpleFusion.inoposuňte doprava →

/*
 * SimpleFusion.ino — knihovna Voltino TriSense 1.5.0
 * Jednoducha fuze senzoru: integrace gyroskopu.
 */

#include <TriSense.h>

TriSense sensor;
SimpleTriFusion fusion(&sensor.imu, &sensor.mag);

unsigned long lastPrint = 0;
const unsigned long printInterval = 50000;   // výstup 20 Hz

void setup() {
  Serial.begin(115200);
  delay(500);

  // Hybridní režim: AK09918C a BMP580 na I2C, ICM42688-P na SPI (CS 17)
  if (!sensor.beginAll(MODE_HYBRID, 17, 10000000)) {
    Serial.println("Failed to initialize sensors!");
    while (1);
  }

  sensor.imu.setODR(ODR_4KHZ);

  Serial.println("Calibrating Gyro... Keep still!");
  sensor.autoCalibrateGyro(500);

  // Kalibrace magnetometru z MotionCalu
  fusion.setMagHardIron(-46.02, -0.85, -46.00);

  float softIron[3][3] = {
    { 0.965, 0.008, -0.002},
    { 0.008, 0.981,  0.139},
    {-0.002, 0.139,  1.077}
  };
  fusion.setMagSoftIron(softIron);
  fusion.setDeclination(5.6);

  Serial.println("Calibrating initial orientation...");
  fusion.initOrientation();
}

void loop() {
  if (fusion.update()) {
    unsigned long now = micros();

    if (now - lastPrint >= printInterval) {
      lastPrint = now;

      float roll, pitch, yaw;
      fusion.getOrientationDegrees(roll, pitch, yaw);

      Serial.print("R: ");    Serial.print(roll, 1);
      Serial.print("\tP: ");  Serial.print(pitch, 1);
      Serial.print("\tY: ");  Serial.println(yaw, 1);
    }
  }
}

Serial Monitor · 115200 baud

Calibrating Gyro... Keep still!
Calibrating initial orientation...
R: -0.4   P: 1.2   Y: 158.7
R: -0.3   P: 1.2   Y: 158.9

4Metoda 2: AdvancedTriFusion

Komplementární filtr s dynamickými zisky. Kombinuje silné stránky všech tří senzorů, takže drží stabilitu i dlouhodobě.

Krátkodobá přesnost: orientaci primárně sleduje gyroskop integrací.
Dlouhodobá korekce: akcelerometr a magnetometr fungují jako stabilní reference, které chybu gyroskopu průběžně opravují — accel koriguje roll a pitch, magnetometr yaw.
Dynamické zisky: filtr sám mění, nakolik korekčním senzorům v dané chvíli věří. Při prudkém pohybu akcelerometru nevěří skoro vůbec, v klidu naopak hodně.

AdvancedFusion.inoposuňte doprava →

/*
 * AdvancedFusion.ino — knihovna Voltino TriSense 1.5.0
 * Pokrocila fuze: komplementarni filtr s Gaussovymi zisky,
 * dynamicke vyprazdnovani FIFO a prubezne uceni driftu gyroskopu.
 *
 * HW: Raspberry Pi Pico 2 / ESP32 + Voltino TriSense
 */

#include <TriSense.h>

TriSense sensor;
AdvancedTriFusion fusion(&sensor.imu, &sensor.mag);

unsigned long lastPrint = 0;
const unsigned long printInterval = 20000;   // výstup 50 Hz

void setup() {
  Serial.begin(115200);
  delay(500);

  // 1. Inicializace (hybridní režim: AK/BMP na I2C, ICM na SPI CS17)
  if (!sensor.beginAll(MODE_HYBRID, 17, 10000000)) {
    Serial.println("Sensor init failed!");
    while (1);
  }

  // Volitelně: 20bitové FIFO s vysokým rozlišením.
  // Fúzní algoritmus se vyšší přesnosti přizpůsobí sám.
  sensor.imu.setFIFOMode(FIFO_20BIT_HIRES);

  // 2. KALIBRACE (vložte své hodnoty)
  sensor.imu.setAccelOffset(0.00, 0.00, 0.00);
  sensor.imu.setAccelScale(1.00, 1.00, 1.00);

  Serial.println("Calibrating Gyro... Keep still!");
  sensor.autoCalibrateGyro(1000);

  fusion.setMagHardIron(-46.02, -0.85, -46.00);

  float softIron[3][3] = {
    { 0.965, 0.008, -0.002},
    { 0.008, 0.981,  0.139},
    {-0.002, 0.139,  1.077}
  };
  fusion.setMagSoftIron(softIron);
  fusion.setDeclination(5.6);

  // Prubezne uceni klidoveho driftu gyroskopu za chodu. Naucena
  // hodnota je omezena (setMaxGyroBias, vychozi 5 dps), takze
  // spatna korekce z magnetometru nemuze integrator rozhodit.
  fusion.setDynamicGyroBias(true, 0.0001f);
  fusion.setMaxGyroBias(5.0f);

  Serial.println("Calibrating initial orientation...");
  fusion.initOrientation();

  Serial.println("Calibration Done, System Running.");
}

void loop() {
  // update() sám vyprázdní FIFO a opraví dt v reálném čase
  if (fusion.update()) {
    unsigned long now = micros();

    if (now - lastPrint >= printInterval) {
      lastPrint = now;

      float roll, pitch, yaw;
      fusion.getOrientationDegrees(roll, pitch, yaw);

      Serial.print("Roll: ");     Serial.print(roll, 1);
      Serial.print(" | Pitch: "); Serial.print(pitch, 1);
      Serial.print(" | Yaw: ");   Serial.println(yaw, 1);
    }
  }
}

Serial Monitor · 115200 baud

Calibration Done, System Running.
Roll: -0.2 | Pitch: 1.1 | Yaw: 159.3
Roll: -0.2 | Pitch: 1.1 | Yaw: 159.3

5Srovnání obou metod

Tabulku posuňte doprava →

Parametr	SimpleTriFusion	AdvancedTriFusion
Princip	integrace gyroskopu	integrace gyroskopu + korekce z accelu a magnetometru
Stabilita yaw	nízká, rychlý drift	vysoká, korigováno magnetometrem
Potlačení šumu	žádné, šum se sčítá	výborné, dynamické zisky
Nutná kalibrace	částečně gyro offset	všech 9 os
Nároky na desku	zvládne i Arduino Uno	ideálně Pico 2 nebo ESP32
Vhodné pro	krátká relativní měření (do 1 s)	dlouhodobé sledování 3D orientace

6Tipy a řešení problémů

Yaw ujíždí: zkontrolujte kalibraci magnetometru (hard-iron i soft-iron) a offsety gyroskopu. Nejčastější příčina je nedokončená kalibrace v MotionCalu.
Nekonzistentní roll a pitch: ověřte kalibraci akcelerometru a hlavně to, že modul byl během inicializace úplně v klidu.
Šum v datech: u AdvancedTriFusion zkuste upravit dynamické Gaussovy zisky přes setAccelGaussian() nebo setMagGaussian().
Odchylky při rychlém pohybu: mírná nepřesnost během prudkých manévrů je fyzikální limit snímačů, ne chyba nastavení.
Orientace se neaktualizuje: zkontrolujte, že COM port neblokuje jiná aplikace a že Serial Monitor běží na 115200 baud.

Stavíte s TriSense něco zajímavého?

Rádi se podíváme, na čem pracujete — a když něco nefunguje, poradíme se zapojením i s laděním fúze.

Voltino TriSense Pro Napsat nám

Modul

Detail modulu →Knihovna na GitHubu Napsat s dotazem

Další návody

První zapojení Voltino BMP580 TriSense Pro: gyroskop a akcelerometr TriSense Pro: magnetometr a kurz k severu Přehled všech návodů →