Upload files to "Pimoroni_icm20948"

Pimoroni_icm20948/Pimoroni_icm20948.ino

@@ -0,0 +1,87 @@
+#include "ICM_20948.h"
+
+#define SERIAL_PORT Serial
+#define WIRE_PORT Wire
+#define I2C_ADDR 0  // ICM-20948 default I2C address (0x68 when AD0 = 0)
+
+ICM_20948_I2C myICM;
+
+// Magnetometer calibration variables
+float mag_min_x = 1000, mag_max_x = -1000;
+float mag_min_y = 1000, mag_max_y = -1000;
+float mag_min_z = 1000, mag_max_z = -1000;
+float mag_offset_x = 0, mag_offset_y = 0, mag_offset_z = 0;
+
+void calibrateMagnetometer() {
+    SERIAL_PORT.println("Calibrating magnetometer... Rotate sensor in all directions for 30 seconds!");
+    unsigned long startTime = millis();
+
+    while (millis() - startTime < 30000) {  // 30 seconds calibration
+        if (myICM.dataReady()) {
+            myICM.getAGMT();
+            float mx = myICM.magX();
+            float my = myICM.magY();
+            float mz = myICM.magZ();
+
+            if (mx < mag_min_x) mag_min_x = mx;
+            if (mx > mag_max_x) mag_max_x = mx;
+            if (my < mag_min_y) mag_min_y = my;
+            if (my > mag_max_y) mag_max_y = my;
+            if (mz < mag_min_z) mag_min_z = mz;
+            if (mz > mag_max_z) mag_max_z = mz;
+        }
+        delay(100);
+    }
+
+    // Compute offsets
+    mag_offset_x = (mag_min_x + mag_max_x) / 2.0;
+    mag_offset_y = (mag_min_y + mag_max_y) / 2.0;
+    mag_offset_z = (mag_min_z + mag_max_z) / 2.0;
+
+    SERIAL_PORT.println("Calibration complete!");
+    SERIAL_PORT.printf("Offsets -> X: %.2f, Y: %.2f, Z: %.2f\n", mag_offset_x, mag_offset_y, mag_offset_z);
+}
+
+void setup() {
+    SERIAL_PORT.begin(115200);
+    while (!SERIAL_PORT);
+
+    WIRE_PORT.begin();
+    WIRE_PORT.setClock(400000);
+
+    while (myICM.begin(WIRE_PORT, I2C_ADDR) != ICM_20948_Stat_Ok) {
+        SERIAL_PORT.println("Error: Unable to initialize ICM-20948! Check wiring.");
+        delay(500);
+    }
+
+    SERIAL_PORT.println("ICM-20948 Initialized.");
+    calibrateMagnetometer();
+}
+
+void loop() {
+    if (myICM.dataReady()) {
+        myICM.getAGMT();
+        float acc_x = myICM.accX() * 9.81 / 1e3;
+        float acc_y = myICM.accY() * 9.81 / 1e3;
+        float acc_z = myICM.accZ() * 9.81 / 1e3;
+        float gyro_x = myICM.gyrX();
+        float gyro_y = myICM.gyrY();
+        float gyro_z = myICM.gyrZ();
+        float temperature = myICM.temp();
+
+        // Apply magnetometer calibration offsets
+        float mag_x = myICM.magX() - mag_offset_x;
+        float mag_y = myICM.magY() - mag_offset_y;
+        float mag_z = myICM.magZ() - mag_offset_z;
+
+        // Compute heading
+        float heading = atan2(mag_y, mag_x) * 180.0 / PI;
+        if (heading < 0) heading += 360;
+
+        SERIAL_PORT.printf("Heading: %6.2f°\t", heading);
+        SERIAL_PORT.printf("Gyro (°/s) X: %7.2f, Y: %7.2f, Z: %7.2f\t", gyro_x, gyro_y, gyro_z);
+        SERIAL_PORT.printf("Accel (m/s²) X: %7.3f, Y: %7.3f, Z: %7.3f\t", acc_x, acc_y, acc_z);
+        SERIAL_PORT.printf("Temp (C) %7.3f\n", temperature);
+    }
+    delay(100);
+}