Basic support for QMC5883L compass

src/main.cpp

@@ -79,6 +79,11 @@
 #include <LiLyGo.h>
 #endif // end LILYGO
 
+#include <heading.h>
+
+DroneHeading droneHeading;
+Compass *compass = NULL;
+
 #define BT_SCAN_DELAY 60 * 1 * 1000
 #define WF_SCAN_DELAY 60 * 2 * 1000
 long noDevicesMillis = 0, cycleCnt = 0;
@@ -1584,6 +1589,22 @@ void setup(void)
 
 #endif
 
+    compass = new QMC5883LCompass();
+    if (!compass->begin())
+    {
+        Serial.println("Failed to initialize Compass");
+    }
+
+    String err = compass->selfTest();
+    if (err.startsWith("OK\n"))
+    {
+        Serial.printf("Compass self-test passed: %s\n", err.c_str());
+    }
+    else
+    {
+        Serial.printf("Compass self-sets failed: %s\n", err.c_str());
+    }
+
 #ifdef UPTIME_CLOCK
     uptime = new UptimeClock(display, millis());
 #endif
@@ -1964,6 +1985,9 @@ void sendMessage(RoutedMessage &m)
 #endif
             }
             break;
+        case HEADING:
+            droneHeading.setHeading(millis(), m.message->payload.heading.heading);
+            break;
         }
     }
 
@@ -2188,6 +2212,12 @@ void loop(void)
         delete mess.message;
     }
 
+    if (compass != NULL)
+    {
+        int16_t heading = compass->heading();
+        Serial.printf("Heading: %" PRIi16 "\n", heading);
+    }
+
     if (!config.is_host)
     {
         doScan();