TriSense Pro: magnetometr a kurz k severu

/*
  AK09918C Magnetometer Example
  -----------------------------
  Čtení a kalibrace dat z 3osého magnetometru AK09918C.
  Aplikuje hard-iron offset i soft-iron korekční matici
  a počítá kurz opravený o lokální magnetickou deklinaci.

  Autor: Tomas Michal / VoltinoLabs
  Licence: MIT
*/

#include <Wire.h>
#include <TriSense.h>
#include <math.h>

AK09918C mag;

// --- Hard-iron offset (statická korekce) --- [µT] X, Y, Z
float magOffset[3] = {-44.96, 9.21, -36.31};

// --- Soft-iron korekční matice (3x3) ---
float magSoftIron[3][3] = {
  { 0.961, 0.004, -0.018},
  { 0.004, 0.998,  0.128},
  {-0.018, 0.128,  1.059}
};

// --- Očekávaná velikost pole (kontrola kalibrace) ---
float magFieldNorm = 49.72;      // [µT]

// --- Lokální magnetická deklinace (kladná = východní) ---
float declinationAngle = 5.02;   // ČR 2025 = +5,02° E

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

  Serial.println("Starting AK09918C example...");

  if (!mag.beginI2C()) {
    Serial.println("Error: AK09918C not found!");
    while (1);
  }

  // Rychlost cteni 100 Hz. Dalsi moznosti: AK_ODR_10HZ, AK_ODR_20HZ, AK_ODR_50HZ.
  // Prefix AK_ je dulezity: pres TriSense.h se holy nazev ODR_100HZ
  // preklada na konstantu IMU a sketch by se nezkompiloval.
  mag.setODR(AK_ODR_100HZ);

  Serial.println("AK09918C initialized.");
}

void loop() {
  if (mag.readData()) {
    // --- Odečtení hard-iron offsetu ---
    float x = mag.x - magOffset[0];
    float y = mag.y - magOffset[1];
    float z = mag.z - magOffset[2];

    // --- Aplikace soft-iron matice ---
    float mx = magSoftIron[0][0]*x + magSoftIron[0][1]*y + magSoftIron[0][2]*z;
    float my = magSoftIron[1][0]*x + magSoftIron[1][1]*y + magSoftIron[1][2]*z;
    float mz = magSoftIron[2][0]*x + magSoftIron[2][1]*y + magSoftIron[2][2]*z;

    // --- Velikost magnetického pole ---
    float magStrength = sqrt(mx*mx + my*my + mz*mz);

    // --- Kurz vůči magnetickému severu ---
    float heading = atan2(my, mx) * 180.0 / PI;
    if (heading < 0) heading += 360.0;

    // --- Oprava na zeměpisný sever ---
    heading += declinationAngle;
    if (heading >= 360.0) heading -= 360.0;

    Serial.print("MX: ");   Serial.print(mx, 2);
    Serial.print("  MY: "); Serial.print(my, 2);
    Serial.print("  MZ: "); Serial.print(mz, 2);
    Serial.print("  | Mag: "); Serial.print(magStrength, 2);
    Serial.print(" uT  | Heading (true north): ");
    Serial.print(heading, 1);
    Serial.println(" deg");

    // --- Kontrola kvality kalibrace ---
    float deviation = fabs(magStrength - magFieldNorm);
    if (deviation > 5) {
      Serial.print("! Warning: magnetic field deviation: ");
      Serial.print(deviation, 2);
      Serial.println(" uT");
    }
  }

  delay(10);   // 100 Hz
}

/*
  AK09918C Magnetometer — MotionCal Example
  ------------------------------------------
  Posílá data magnetometru do nástroje MotionCal,
  který z nich spočítá hard-iron offsety a soft-iron matici.

  Postup:
  1. Nahrajte tento sketch na Arduino.
  2. Zavřete Serial Monitor v Arduino IDE.
  3. Spusťte MotionCal, vyberte COM port a dejte Start.
  4. Otáčejte deskou plynule ve všech osách, dokud se
     nevykreslí koule a "Gaps" neklesnou pod 2 %.
  5. Zapište si vypsané offsety a matici.

  Poznámka: posíláme jen data magnetometru; hodnoty
  akcelerometru a gyroskopu jsou nulové kvůli kompatibilitě
  s formátem, který MotionCal očekává.
*/

#include <Wire.h>
#include <TriSense.h>

AK09918C mag;

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

  if (!mag.beginI2C()) {
    Serial.println("AK09918C not found!");
    while (1);
  }

  // Prefix AK_ je nutny — viz poznamka v predchozim sketchi.
  mag.setODR(AK_ODR_100HZ);
  Serial.println("AK09918C initialized!");
}

void loop() {
  if (mag.readData()) {
    // MotionCal očekává 9 hodnot: ax, ay, az, gx, gy, gz, mx, my, mz
    Serial.print("Raw:0,0,0,0,0,0,");
    Serial.print(int(mag.x * 10)); Serial.print(",");
    Serial.print(int(mag.y * 10)); Serial.print(",");
    Serial.println(int(mag.z * 10));

    Serial.print("Uni:0.0,0.0,0.0,0.0,0.0,0.0,");
    Serial.print(mag.x / 1000.0, 4); Serial.print(",");
    Serial.print(mag.y / 1000.0, 4); Serial.print(",");
    Serial.println(mag.z / 1000.0, 4);
  }

  delay(10);   // 100 Hz
}