diff --git a/lib/heading/Compass.cpp b/lib/heading/Compass.cpp new file mode 100644 index 0000000..3fa5309 --- /dev/null +++ b/lib/heading/Compass.cpp @@ -0,0 +1,284 @@ +#include "heading.h" +#include + +/* + * 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; +} diff --git a/lib/heading/DroneHeading.cpp b/lib/heading/DroneHeading.cpp new file mode 100644 index 0000000..475184b --- /dev/null +++ b/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; } diff --git a/lib/heading/heading.h b/lib/heading/heading.h new file mode 100644 index 0000000..d3bebbb --- /dev/null +++ b/lib/heading/heading.h @@ -0,0 +1,96 @@ +#pragma once +#include +#include + +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 diff --git a/src/main.cpp b/src/main.cpp index 68b455b..ca42b53 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -79,6 +79,11 @@ #include #endif // end LILYGO +#include + +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();