diff --git a/lib/comms/comms.cpp b/lib/comms/comms.cpp index b4824b7..b31fe13 100644 --- a/lib/comms/comms.cpp +++ b/lib/comms/comms.cpp @@ -184,19 +184,18 @@ String _scan_str(ScanTask &); String _scan_result_str(ScanTaskResult &); String _wrap_str(String); -#define POLY 0x1021 -uint16_t crc16(String v, uint16_t c) +uint16_t crc16(uint16_t poly, uint16_t c, size_t sz, uint8_t *v) { c ^= 0xffff; - for (int i = 0; i < v.length(); i++) + for (int i = 0; i < sz; i++) { - uint16_t ch = v.charAt(i); + uint16_t ch = v[i]; c = c ^ (ch << 8); for (int j = 0; j < 8; j++) { if (c & 0x8000) { - c = (c << 1) ^ POLY; + c = (c << 1) ^ poly; } else { @@ -208,6 +207,12 @@ uint16_t crc16(String v, uint16_t c) return c ^ 0xffff; } +#define POLY 0x1021 +uint16_t crc16(String v, uint16_t c) +{ + return crc16(POLY, c, v.length(), (uint8_t *)v.c_str()); +} + void ReadlineComms::_onReceive() { while (serial.available() > 0) diff --git a/lib/comms/comms.h b/lib/comms/comms.h index 2b5124a..4154e36 100644 --- a/lib/comms/comms.h +++ b/lib/comms/comms.h @@ -90,6 +90,7 @@ struct Endpoint { union { + struct { uint8_t loop : 1, // self @@ -158,14 +159,35 @@ extern Comms *Comms0; extern Comms *Comms1; +struct LoRaStats +{ + uint64_t t0; + int64_t rssi_60; + int64_t snr_60; + + int16_t last_rssi; + int16_t last_snr; + + int64_t messages_60; + int64_t errors_60; + + LoRaStats() + : t0(0), rssi_60(0), snr_60(0), last_rssi(0), last_snr(0), messages_60(0), + errors_60(0) + { + } +}; + struct RadioComms { String name; RADIO_TYPE &radio; LoRaConfig &loraCfg; + LoRaStats stats; + RadioComms(String name, RADIO_TYPE &radio, LoRaConfig &cfg) - : name(name), radio(radio), loraCfg(cfg) + : name(name), radio(radio), loraCfg(cfg), stats() { } @@ -176,6 +198,15 @@ struct RadioComms Message *receive(uint16_t timeout_ms); }; +uint16_t crc16(uint16_t poly, uint16_t c, size_t sz, uint8_t *v); + +/* + * Given halflife (i.e. time it takes the accumulator to decay to 50%), compute + * the updated cumulate at new time. That is, acc_now = decay(acc_t, inc). + */ +int64_t updateExpDecay(uint16_t halflife, int64_t acc, uint64_t t, uint64_t now, + int64_t inc); + extern RadioComms *RxComms; extern RadioComms *TxComms; diff --git a/lib/comms/radio_comms.cpp b/lib/comms/radio_comms.cpp index 488e09f..2048359 100644 --- a/lib/comms/radio_comms.cpp +++ b/lib/comms/radio_comms.cpp @@ -188,6 +188,11 @@ int16_t RadioComms::send(Message &m) size_t p = MAX_MSG; uint8_t *msg = NULL; + if (loraCfg.crc) + { + p -= 2; + } + if (m.type == SCAN_RESULT) { msg = _serialize_scan_result(m, p, msg_buf); @@ -207,6 +212,13 @@ int16_t RadioComms::send(Message &m) return RADIOLIB_ERR_INVALID_FUNCTION; } + if (loraCfg.crc) + { + uint16_t c = loraCfg.crc_seed ^ 0xffff; + c = crc16(loraCfg.crc_poly, c, p, msg); + _write(msg, MAX_MSG, p, (uint8_t *)&c, 2); + } + int16_t status = radio.transmit(msg, p); if (msg != msg_buf) @@ -385,7 +397,7 @@ Message *RadioComms::receive(uint16_t timeout_ms) radio.clearDio1Action(); packetRssi = radio.getRSSI(true); - Serial.println("LORA_RSSI: " + String(packetRssi)); + // Serial.println("LORA_RSSI:" + String(packetRssi)); size_t len = radio.getPacketLength(true); uint8_t *packet = msg; diff --git a/lib/config/config.cpp b/lib/config/config.cpp index 469de55..28c8779 100644 --- a/lib/config/config.cpp +++ b/lib/config/config.cpp @@ -189,7 +189,9 @@ String loraConfigToStr(LoRaConfig *cfg) String(",tx_power:") + String(cfg->tx_power) + String(",preamble_len:") + String(cfg->preamble_len) + String(",sync_word:") + String(cfg->sync_word, 16) + String(",crc:") + String(cfg->crc ? "1" : "0") + - String(",implicit_header:") + String(cfg->implicit_header); + String(",crc_seed:") + String(cfg->crc_seed, 16) + String(",crc_poly:") + + String(cfg->crc_poly, 16) + String(",implicit_header:") + + String(cfg->implicit_header); } String detectionStrategyToStr(Config &c) @@ -356,6 +358,8 @@ LoRaConfig *configureLora(String cfg) preamble_len : 8, sync_word : 0x1e, crc : false, + crc_seed : 0, + crc_poly : 0x1021, implicit_header : 0 }); @@ -383,6 +387,18 @@ LoRaConfig *configureLora(String cfg) continue; } + if (k.equalsIgnoreCase("crc_seed")) + { + lora->crc_seed = (uint16_t)fromHex(param); + continue; + } + + if (k.equalsIgnoreCase("crc_poly")) + { + lora->crc_poly = (uint16_t)fromHex(param); + continue; + } + if (k.equalsIgnoreCase("freq")) { lora->freq = param.toFloat(); diff --git a/lib/config/config.h b/lib/config/config.h index c09107b..b52e740 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -46,6 +46,8 @@ struct LoRaConfig uint16_t preamble_len; uint8_t sync_word; bool crc; + uint16_t crc_seed; + uint16_t crc_poly; uint8_t implicit_header; }; 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 ba25bbb..5e69d34 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -139,6 +139,11 @@ void sendBTData(float heading, float rssi) #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; @@ -690,6 +695,7 @@ void osdProcess() Config config; +#ifdef USING_LR1121 void setLRFreq(float freq) { bool skipCalibration = false; @@ -715,6 +721,7 @@ void setLRFreq(float freq) radio.freqMHz = freq; radio.highFreq = (freq > 1000.0); } +#endif float getRSSI(void *param) { @@ -1646,6 +1653,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 @@ -2026,6 +2049,9 @@ void sendMessage(RoutedMessage &m) #endif } break; + case HEADING: + droneHeading.setHeading(millis(), m.message->payload.heading.heading); + break; } } @@ -2135,6 +2161,7 @@ void doScan(); void reportScan(); +#ifdef COMPASS_ENABLED float getCompassHeading() { /* code */ @@ -2225,6 +2252,7 @@ float getCompassHeading() float headingDegrees = heading * 180 / M_PI; return headingDegrees; } +#endif float historicalCompassRssi[STEPS] = {999}; int compassCounter = 0; @@ -2248,6 +2276,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();