Basic support for QMC5883L compass

lib/heading/Compass.cpp

@@ -0,0 +1,284 @@
+#include "heading.h"
+#include <Wire.h>
+
+/*
+ * QMC5883L Registers:
+ *
+ * 00...05 Data Output registers
+ * 00: X[7:0] (X LSB)
+ * 01: X[15:8] (X MSB)
+ * 02: Y[7:0] (Y LSB)
+ * 03: Y[15:8] (Y MSB)
+ * 04: Z[7:0] (Z LSB)
+ * 05: Z[15:8] (Z MSB)
+ *
+ * 06 Status:
+ *      |  7  |  6  |  5  |  4  |  3  |  2  |  1  |  0   |
+ *      |      Reserved               | DOR | OVL | DRDY |
+ *
+ * DRDY - Data Ready, set to 1 when all three axis data is ready and loaded in
+ * the continuous measurement mode, and it is reset to 0 upon reading any of the
+ * registers 00...05
+ *
+ * OVL - Overflow, set to 1 when any data for any axis of the magnetic sensor is
+ * out of range. The output data saturates at -32768 and 32767. If any of the
+ * axis exceeds the range, OVL is set to 1.
+ *
+ * DOR - Data Skip is set to 1, if all the channels of output data registers are
+ * skipped in the continuous-measurement mode. It is set back to 0 by reading
+ * data from registers 00...05
+ *
+ * 07, 08 Temperature Data
+ * 07: TOUT[7:0]
+ * 08: TOUT[15:8]
+ *
+ * 09 Control:
+ *      |  7  |  6  |  5  |  4  |  3  |  2  |  1  |  0  |
+ *      |  OSR[1:0] | RNG[1:0]  | ODR[1:0]  | MODE[1:0] |
+ *
+ *                     | 00      | 01         |   10  |   11  |
+ * Mode | Mode Control | Standby | Continuous |   Reserved    |
+ *      |              |         |            |               |
+ * ODR  | Output Data  | 10Hz    | 50Hz       | 100Hz | 200Hz |
+ *      | Rate         |         |            |       |       |
+ * RNG  | Full Scale   | +/-2G   | +/-8G      |   Reserved    |
+ *      |              |         |            |               |
+ * OSR  | Over Sample  |   512   |    256     |  128  |  64   |
+ *      | Ratio        |         |            |       |       |
+ *
+ * - Recommended RNG=+/-2G for open field, for better energy saving
+ * - Recommended ODR=10Hz for plain compass
+ *
+ *
+ * 0A Control2:
+ *      |     7     |     6     |  5  |  4  |  3  |  2  |  1  |    0    |
+ *      |  SOFT_RST | ROLL_PNT  |          Reserved           | INT_ENB |
+ *
+ * 0B Set/Reset period:
+ * FBR[7:0]. Recommended to set this to 1
+ */
+
+bool QMC5883LCompass::begin()
+{
+    _lastErr = CompassStatus::COMPASS_UNINITIALIZED;
+
+    if (!Wire1.begin(46, 42)) /* SDA, SCL */
+    {
+        return false;
+    }
+
+    String err = selfTest();
+    if (!err.startsWith("OK\n"))
+    {
+        return false;
+    }
+
+    _lastErr = CompassStatus::COMPASS_OK;
+    return true;
+}
+
+uint8_t _write_register(uint8_t addr, uint8_t reg, uint8_t value, bool skipValue = false)
+{
+    Wire1.beginTransmission(addr);
+    size_t s = Wire1.write(reg);
+    if (s == 1 && !skipValue)
+    {
+        s = Wire1.write(value);
+    }
+
+    size_t s1 = Wire1.endTransmission();
+    if (s != 1 && s1 == 0)
+    {
+        return 1; // "data too long to fit in transmit buffer"
+    }
+
+    return s1;
+}
+
+int8_t _read_registers(uint8_t addr, uint8_t reg, uint8_t *v, size_t sz,
+                       bool skipRegister = false)
+{
+    if (!skipRegister)
+    {
+        uint8_t s = _write_register(addr, reg, 0, true);
+        if (s != 0)
+        {
+            return s;
+        }
+    }
+
+    uint8_t r = Wire1.requestFrom(addr, sz);
+    for (int i = 0; i < r; i++, v++)
+    {
+        *v = Wire1.read();
+    }
+
+    return r - sz;
+}
+
+uint8_t _read_register(uint8_t addr, uint8_t reg, uint8_t &v, bool skipRegister = false)
+{
+    uint8_t r = _read_registers(addr, reg, &v, 1, skipRegister);
+    if (r != 0)
+    {
+        return 1;
+    }
+
+    return 0;
+}
+
+int8_t _read_xyz(CompassXYZ &xyz)
+{
+    xyz.status = 0;
+    size_t s = _read_register(QMC5883_ADDR, QMC5883_STATUS_REG, xyz.status);
+    if (s != 0)
+    {
+        return s;
+    }
+
+    if ((xyz.status & QMC5883_STATUS_DRDY) == 0)
+    {
+        delay(10);
+        s = _read_register(QMC5883_ADDR, QMC5883_STATUS_REG, xyz.status);
+        if (s != 0)
+        {
+            return s;
+        }
+    }
+
+    int16_t mags[3];
+
+    int8_t r = _read_registers(QMC5883_ADDR, 0, (uint8_t *)&mags, 6);
+    xyz.x = mags[0];
+    xyz.y = mags[1];
+    xyz.z = mags[1];
+
+    return r;
+}
+
+uint8_t QMC5883LCompass::setMode(CompassMode m)
+{
+    if (m == CompassMode::COMPASS_IDLE)
+    {
+        uint8_t s = _write_register(QMC5883_ADDR, QMC5883_FBR_REG, 0);
+        s |= _write_register(QMC5883_ADDR, QMC5883_CTR_REG, 0);
+        return s;
+    }
+
+    uint8_t osr = 0b00;  // OSR=512
+    uint8_t rng = 0b01;  // RNG = +/-8 Gauss
+    uint8_t odr = 0b11;  // ODR = 200HZ
+    uint8_t mode = 0b01; // MODE = Continuous
+
+    switch (m)
+    {
+    case CompassMode::CONTINUOUS_PERF_HIGH_GAIN:
+        break;
+    case CompassMode::CONTINUOUS_EFF_HIGH_GAIN:
+        odr = 0;
+        break;
+    case CompassMode::CONTINUOUS_EFF_LOW_GAIN:
+        odr = 0;
+        rng = 0;
+        break;
+    default:
+        return 1;
+    }
+
+    uint8_t s = _write_register(QMC5883_ADDR, QMC5883_FBR_REG, 1); // set/reset period
+    if (s != 0)
+    {
+        return s;
+    }
+
+    return _write_register(QMC5883_ADDR, QMC5883_CTR_REG,
+                           (osr << 6) | (rng << 4) | (odr << 2) | mode);
+}
+
+String QMC5883LCompass::selfTest()
+{
+    uint8_t s = setMode(CompassMode::CONTINUOUS_PERF_HIGH_GAIN);
+    if (s != 0)
+    {
+        return String("Could not set Compass");
+    }
+
+    delay(100);
+
+    bool errors = false;
+
+    String res;
+    for (int i = 0; i < 100; i++)
+    {
+        CompassXYZ xyz;
+        int8_t r = _read_xyz(xyz);
+        if (r < 0)
+        {
+            errors = true;
+            res += " oops, requested 6, got back only " + String(-r);
+            continue;
+        }
+
+        if (r != 0)
+        {
+            errors = true;
+            res += "\nCould not get XYZ: err:" + String(r);
+            continue;
+        }
+
+        res += "\n status: " + String(xyz.status, 2) + " X: " + String(xyz.x) +
+               " Y: " + String(xyz.y) + " Z: " + String(xyz.z);
+    }
+
+    if (!errors)
+    {
+        res = "OK\n" + res;
+    }
+
+    return res;
+}
+
+int8_t QMC5883LCompass::readXYZ() { return _read_xyz(xyz); }
+
+int64_t Compass::lastRead()
+{
+    if (_lastErr == CompassStatus::COMPASS_UNINITIALIZED)
+    {
+        return -1;
+    }
+    return _lastRead;
+}
+
+int16_t Compass::heading()
+{
+    if (_lastErr == CompassStatus::COMPASS_UNINITIALIZED)
+    {
+        return -999;
+    }
+
+    _lastRead = millis();
+
+    int8_t r = readXYZ();
+
+    if (r != 0)
+    {
+        return -999;
+    }
+
+    float heading = atan2(xyz.y, xyz.x);
+    float declinationAngle = 0.22;
+    heading += declinationAngle;
+
+    // Correct for when signs are reversed.
+    if (heading < 0)
+        heading += 2 * M_PI;
+
+    // Check for wrap due to addition of declination.
+    if (heading > 2 * M_PI)
+        heading -= 2 * M_PI;
+
+    // Convert radians to degrees for readability.
+    float headingDegrees = heading * 180 / M_PI;
+
+    return headingDegrees;
+}

lib/heading/DroneHeading.cpp

@@ -0,0 +1,10 @@
+#include "heading.h"
+
+void DroneHeading::setHeading(int64_t now, int16_t h)
+{
+    _heading = h;
+    _lastRead = now;
+}
+
+int64_t DroneHeading::lastRead() { return _lastRead; }
+int16_t DroneHeading::heading() { return _heading; }

lib/heading/heading.h

@@ -0,0 +1,96 @@
+#pragma once
+#include <WString.h>
+#include <cstdint>
+
+struct HeadingSensor
+{
+    HeadingSensor() {}
+
+    virtual int64_t lastRead() = 0;
+    virtual int16_t heading() = 0;
+};
+
+enum CompassMode
+{
+    COMPASS_IDLE,
+    CONTINUOUS_PERF_HIGH_GAIN, // high-performance, high gain
+    CONTINUOUS_EFF_HIGH_GAIN,  // energy-saving, high gain
+    CONTINUOUS_EFF_LOW_GAIN    // energy-saving, low gain
+};
+
+enum CompassStatus
+{
+    COMPASS_OK = 0,
+    COMPASS_DATA_TOO_LONG = 1,
+    COMPASS_NACK_ADDR = 2,
+    COMPASS_NACK_DATA = 3,
+    COMPASS_OTHER_ERR = 4,
+    COMPASS_TIMEOUT = 5,
+    COMPASS_UNINITIALIZED = 6
+};
+
+struct CompassXYZ
+{
+    uint8_t status;
+    int16_t x;
+    int16_t y;
+    int16_t z;
+};
+
+struct Compass : HeadingSensor
+{
+    int8_t _lastErr; // if less than 0, it's a read error; otherwise it's CompassStatus
+    int64_t _lastRead;
+    CompassXYZ xyz;
+
+    Compass() : HeadingSensor(), _lastErr(CompassStatus::COMPASS_UNINITIALIZED) {}
+
+    virtual bool begin() = 0;
+    virtual String selfTest() = 0;
+
+    virtual uint8_t setMode(CompassMode m) = 0;
+    virtual int8_t readXYZ() = 0;
+
+    virtual int64_t lastRead() override;
+    virtual int16_t heading() override;
+};
+
+struct QMC5883LCompass : Compass
+{
+    QMC5883LCompass() : Compass() {}
+
+    bool begin() override;
+    String selfTest() override;
+
+    uint8_t setMode(CompassMode m) override;
+    int8_t readXYZ() override;
+};
+
+struct DroneHeading : HeadingSensor
+{
+    int64_t _lastRead;
+    int16_t _heading;
+
+    DroneHeading() : HeadingSensor(), _lastRead(-1) {}
+
+    void setHeading(int64_t now, int16_t heading);
+
+    int64_t lastRead() override;
+    int16_t heading() override;
+};
+
+#define QMC5883_ADDR 0xD
+#define QMC5883_X_LSB_REG 0
+#define QMC5883_X_MSB_REG 1
+#define QMC5883_Y_LSB_REG 2
+#define QMC5883_Y_MSB_REG 3
+#define QMC5883_Z_LSB_REG 4
+#define QMC5883_Z_MSB_REG 5
+#define QMC5883_STATUS_REG 6
+#define QMC5883_T_LSB_REG 7
+#define QMC5883_T_MSB_REG 8
+#define QMC5883_CTR_REG 9
+#define QMC5883_CTR2_REG 0xA
+#define QMC5883_FBR_REG 0xB
+
+#define QMC5883_STATUS_DRDY 1

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();