Added calibration + needle point

doesn't work incredibly well however, but it will point towards an ordinary magnet if close enough

Author: JamesHarcourt7

src/compass.c

@@ -1,10 +1,15 @@
 #include <stdio.h>
 
 #include "pico/stdlib.h"
-#include "lib/fonts.h"
 #include "lib/st7735.h"
 #include "lib/ICM20948.h"
 
+struct Vec {
+    float x;
+    float y;
+    float z;
+};
+
 // Thanks plaaosert for the ST7735 and IMU guide :D
 
 void SetupIMU() {
@@ -36,41 +41,78 @@ int main() {
 
     SetupIMU();
 
-    float x;
-    float y;
-    float z;
-
-    while (true) {
-        icm20948MagRead(&x, &y, &z);
+    struct Vec mag;
+    // For calibration:
+    struct Vec magmin;
+    struct Vec magmax;
 
-        char arrayX[10];
-        if (x > 0) {
-            sprintf(arrayX, "%.2f", x);
-        } else {
-            sprintf(arrayX, "%.1f", x);
-        }
+    int centre_x = ST7735_WIDTH / 2;
+    int centre_y = ST7735_HEIGHT / 2;
+    int point_x;
+    int point_y;
+    bool negative;
+    float magnitude;
+    int current;
+    float heading;
+    char arrayH[10];
 
-        char arrayY[10];
-        if (y > 0) {
-            sprintf(arrayY, "%.2f", y);
-        } else {
-            sprintf(arrayY, "%.1f", y);
-        }
+    icm20948MagRead(&magmin.x, &magmin.y, &magmin.z);
+    icm20948MagRead(&magmax.x, &magmax.y, &magmin.z);
 
-        char arrayZ[10];
-        if (z > 0) {
-            sprintf(arrayZ, "%.2f", z);
+    while (true) {
+        // Read magnetometer values
+        icm20948MagRead(&mag.x, &mag.y, &mag.z);
+
+        // Calibrate values
+        if (mag.x < magmin.x) magmin.x = mag.x;
+        if (mag.x > magmax.x) magmax.x = mag.x;
+        if (mag.y < magmin.y) magmin.y = mag.y;
+        if (mag.y > magmax.y) magmax.y = mag.y;
+        if (mag.z < magmin.z) magmin.z = mag.z;
+        if (mag.z > magmax.z) magmax.z = mag.z;
+
+        // Calibrate by removing offset of smallest seen value
+        mag.x -= magmin.x;
+        mag.y -= magmin.y;
+        mag.z -= magmin.z;
+
+        // Scale value based of highest values seen
+        // Creates values between 0 and 1
+
+        // Avoid Zero Division
+        if (magmax.x - magmin.x > 0.001) mag.x /= magmax.x - magmin.x;
+        if (magmax.y - magmin.y > 0.001) mag.y /= magmax.y - magmin.y;
+        if (magmax.z - magmin.z > 0.001) mag.z /= magmax.z - magmin.z;
+
+        // Shift values to -0.5 <= x <= 0.5
+        mag.x -= 0.5;
+        mag.y -= 0.5;
+        mag.z -= 0.5;
+
+        // Convert Gauss values to Radian heading
+        heading = atan2(mag.x, mag.y);
+        // Convert to positive heading (add a full radian turn)
+        if (heading < 0) heading += 2 * 3.14159;
+
+        // Worked out using counterclockwise rotation matrix
+        point_x = centre_x + (20 * sin(heading));
+        point_y = centre_y + ((-20) * cos(heading));
+
+        // Draw compass needle
+        ST7735_FillScreen(ST7735_BLACK);
+
+        ST7735_DrawPixel(centre_x, centre_y, ST7735_RED);
+        ST7735_DrawPixel(point_x, point_y, ST7735_RED);
+
+        // Convert heading to degrees
+        heading *= 180 / 3.14159;
+
+        if (heading > 0) {
+            sprintf(arrayH, "%.2f", heading);
         } else {
-            sprintf(arrayZ, "%.1f", z);
+            sprintf(arrayH, "%.1f", heading);
         }
-
-        ST7735_WriteString(5, 5, arrayX, Font_16x26, ST7735_RED, ST7735_BLACK);
-        ST7735_WriteString(5, 31, arrayY, Font_16x26, ST7735_GREEN, ST7735_BLACK);
-        ST7735_WriteString(5, 57, arrayZ, Font_16x26, ST7735_BLUE, ST7735_BLACK);
-
-        ST7735_WriteString(7, 120, "x", Font_16x26, ST7735_RED, ST7735_BLACK);
-        ST7735_WriteString(27, 120, "y", Font_16x26, ST7735_GREEN, ST7735_BLACK);
-        ST7735_WriteString(47, 120, "z", Font_16x26, ST7735_BLUE, ST7735_BLACK);
+        ST7735_WriteString(80, 80, arrayH, Font_16x26, ST7735_RED, ST7735_BLACK);
 
         sleep_ms(100);
     }