diff --git a/README.md b/README.md index c27f486..4b0ae35 100644 --- a/README.md +++ b/README.md @@ -527,3 +527,16 @@ Use connector and solder to RPI or similar, I'm using a RPI Zero 2. The targets - `-DSERIAL_OUT` to enable serial output - `-DSEEK_ON_X` to enable seek on jam - `-DBANDWIDTH=4.8` to set the bandwidth to 4.8 kHz + + ### 12. Compass Integration: +Lyligo only + ``` +X DROY(not used) +GPIO46 -> SDA +GPIO42 -> SCLA +GND -> GND +3.3V -> VCC + ``` +Module: GY-271 + +https://www.amazon.com/HiLetgo-GY-271-QMC5883L-Compass-Magnetometer/dp/B008V9S64E diff --git a/lib/comms/bus.cpp b/lib/comms/bus.cpp new file mode 100644 index 0000000..520aa58 --- /dev/null +++ b/lib/comms/bus.cpp @@ -0,0 +1,92 @@ +#include "bus.h" +#include + +bool initUARTs(Config &config) +{ + if (config.uart0.enabled) + { + Uart0.end(); + Uart0.begin(config.uart0.clock_freq, SERIAL_8N1, config.uart0.rx, + config.uart0.tx); + } + + if (config.uart1.enabled) + { + Uart1.end(); + Uart1.begin(config.uart1.clock_freq, SERIAL_8N1, config.uart1.rx, + config.uart1.tx); + } + + return true; +} + +SPIClass &hspi = *(new SPIClass(HSPI)); // not usable until initSPIs + +bool initSPIs(Config &config) +{ + if (config.spi1.enabled) + { + delete (&hspi); + hspi = *(new SPIClass(config.spi1.bus_num)); + if (config.spi1.clock_freq > 0) + { + hspi.setFrequency(config.spi1.clock_freq); + } + + // if all the pins are -1, then will use the default for SPI bus_num + hspi.begin(config.spi1.clk, config.spi1.miso, config.spi1.mosi); + Serial.printf("Initialized SPI%d @ %x: SC:%d MISO:%d MOSI:%d clock:%d\n", + (int)config.spi1.bus_num, (void *)&hspi, (int)config.spi1.clk, + (int)config.spi1.miso, (int)config.spi1.mosi, + (int)config.spi1.clock_freq); + } + + return true; +} + +uint8_t _scanSupportedDevicesOnWire(TwoWire &w, int bus_num); + +uint8_t wireDevices; +uint8_t wire1Devices; + +bool initWires(Config &config) +{ + wireDevices = _scanSupportedDevicesOnWire(Wire, 0); + + if (config.wire1.enabled) + { +#if SOC_I2C_NUM > 1 + // if you want to use default pins, configure -1 + // if you want to use default clock speed, configure 0 + if (!Wire1.begin(config.wire1.sda, config.wire1.scl, config.wire1.clock_freq)) + { + Serial.println("Failed to initialize Wire1"); + return false; + } + + wire1Devices = _scanSupportedDevicesOnWire(Wire1, 1); +#endif + } + + return true; +} + +uint8_t _scanSupportedDevicesOnWire(TwoWire &w, int bus_num) +{ + uint8_t res = 0; + + for (int i = 0; known_i2c_devices[i].address > 0; i++) + { + w.beginTransmission(known_i2c_devices[i].address); + delay(2); + if (w.endTransmission() == 0) + { + Serial.printf("Found supported device on Wire%d: %s(%X)\n", bus_num, + known_i2c_devices[i].name.c_str(), + (int)known_i2c_devices[i].address); + res |= 1 << i; + } + } + + return res; +} diff --git a/lib/comms/bus.h b/lib/comms/bus.h new file mode 100644 index 0000000..b94599e --- /dev/null +++ b/lib/comms/bus.h @@ -0,0 +1,39 @@ +#pragma once +#include + +struct +{ + String name; + uint8_t address; +} known_i2c_devices[] = {{"HMC5883L", 0x1e}, {"QMC5883L", 0x0d}, {" last record ", 0}}; + +enum I2CDevices +{ + // powers of 2 + HMC5883L = 1, + QMC5883L = 2 +}; + +extern uint8_t wireDevices; +extern uint8_t wire1Devices; + +extern SPIClass &hspi; + +// abstract away a reference to Serial vs Serial0 vs Serial1, so it compiles +#ifndef ARDUINO_USB_CDC_ON_BOOT +#define Uart0 Serial +#else +#define Uart0 Serial0 +#endif + +#if SOC_UART_NUM > 1 +#define Uart1 Serial1 +#else +#define Uart1 Uart0 +#endif + +bool initSPIs(Config &config); + +bool initUARTs(Config &config); + +bool initWires(Config &config); diff --git a/lib/comms/comms.cpp b/lib/comms/comms.cpp index b4824b7..c0215d0 100644 --- a/lib/comms/comms.cpp +++ b/lib/comms/comms.cpp @@ -86,9 +86,8 @@ bool Comms::initComms(Config &c) if (c.listen_on_serial0.equalsIgnoreCase("readline")) { // comms using readline plaintext protocol - Comms0 = new ReadlineComms("UART0", SERIAL0); - SERIAL0.onReceive(_onReceive0, false); - SERIAL0.begin(115200); + Comms0 = new ReadlineComms("UART0", Uart0); + Uart0.onReceive(_onReceive0, false); Serial.println("Initialized communications on Serial0 using readline protocol"); } @@ -102,9 +101,8 @@ bool Comms::initComms(Config &c) if (c.listen_on_serial1.equalsIgnoreCase("readline")) { // comms using readline plaintext protocol - Comms1 = new ReadlineComms("UART1", Serial1); - Serial1.onReceive(_onReceive1, false); - Serial1.begin(115200); + Comms1 = new ReadlineComms("UART1", Uart1); + Uart1.onReceive(_onReceive1, false); Serial.println("Initialized communications on Serial1 using readline protocol"); } @@ -184,19 +182,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 +205,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) @@ -415,7 +418,7 @@ String _scan_result_str(ScanTaskResult &r) for (int i = 0; i < r.sz; i++) { p += (i == 0 ? "(" : ", (") + String(r.freqs_khz[i]) + ", " + String(r.rssis[i]) + - ")"; + (r.rssis2 ? ", " + String(r.rssis2[i]) : "") + ")"; } return p + " ]\n"; diff --git a/lib/comms/comms.h b/lib/comms/comms.h index 2b5124a..57fb90b 100644 --- a/lib/comms/comms.h +++ b/lib/comms/comms.h @@ -5,14 +5,9 @@ #include #include +#include #include -#ifndef ARDUINO_USB_CDC_ON_BOOT -#define SERIAL0 Serial -#else -#define SERIAL0 Serial0 -#endif - #ifndef SCAN_MAX_RESULT_KHZ_SCALE // kHz scale: round frequency, so it fits into 2 bytes // 2500000 / 40 = 62500, scale 40 fits 2.5GHz into two bytes @@ -56,6 +51,7 @@ struct ScanTaskResult size_t sz; uint32_t *freqs_khz; int16_t *rssis; + int16_t *rssis2; int16_t prssi; }; @@ -90,6 +86,7 @@ struct Endpoint { union { + struct { uint8_t loop : 1, // self @@ -134,7 +131,7 @@ struct Comms struct NoopComms : Comms { - NoopComms() : Comms("no-op", SERIAL0) {}; + NoopComms() : Comms("no-op", Uart0) {}; virtual bool send(Message &) { return true; }; virtual void _onReceive() {}; @@ -158,14 +155,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 +194,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..3f3d8d9 100644 --- a/lib/config/config.cpp +++ b/lib/config/config.cpp @@ -88,18 +88,6 @@ Config Config::init() continue; } - if (r.key.equalsIgnoreCase("rx_lora")) - { - c.rx_lora = configureLora(r.value); - continue; - } - - if (r.key.equalsIgnoreCase("tx_lora")) - { - c.tx_lora = configureLora(r.value); - continue; - } - Serial.printf("Unknown key '%s' will be ignored\n", r.key); } @@ -170,6 +158,36 @@ bool Config::updateConfig(String key, String value) return true; } + if (key.equalsIgnoreCase("uart0")) + { + uart0 = BusConfig::configure(value); + return true; + } + + if (key.equalsIgnoreCase("uart1")) + { + uart1 = BusConfig::configure(value); + return true; + } + + if (key.equalsIgnoreCase("spi1")) + { + spi1 = BusConfig::configure(value); + return true; + } + + if (key.equalsIgnoreCase("wire1")) + { + wire1 = BusConfig::configure(value); + return true; + } + + if (key.equalsIgnoreCase("radio2")) + { + radio2 = RadioModuleSPIConfig::configure(value); + return true; + } + UPDATE_BOOL(is_host, key, value); UPDATE_BOOL(lora_enabled, key, value); @@ -189,7 +207,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 +376,8 @@ LoRaConfig *configureLora(String cfg) preamble_len : 8, sync_word : 0x1e, crc : false, + crc_seed : 0, + crc_poly : 0x1021, implicit_header : 0 }); @@ -383,6 +405,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(); @@ -483,6 +517,102 @@ void Config::configureDetectionStrategy(String cfg) } } +RadioModuleSPIConfig RadioModuleSPIConfig::configure(String cfg) +{ + RadioModuleSPIConfig c; + + c.bus_num = 1; + c.cs = RADIO_MODULE_CS_PIN; + c.rst = RADIO_MODULE_RST_PIN; + c.dio1 = RADIO_MODULE_DIO1_PIN; + c.busy = RADIO_MODULE_BUSY_PIN; + + c.clock_freq = RADIO_MODULE_CLOCK_FREQ; + c.msb_first = RADIO_MODULE_MSB_FIRST; + c.spi_mode = RADIO_MODULE_SPI_MODE; + + int begin = 0; + int end, i; + + while ((i = findSepa(cfg, ",", begin, end)) >= 0) + { + String param = cfg.substring(begin, end); + begin = i; + int j = param.indexOf(":"); + if (j < 0) + { + c.module = param; + c.enabled = !param.equalsIgnoreCase("none"); + continue; + } + + String k = param.substring(0, j); + param = param.substring(j + 1); + + k.toLowerCase(); + if (k.equals("bus")) + { + c.bus_num = param.toInt(); + continue; + } + + if (k.equals("rst")) + { + c.rst = param.toInt(); + continue; + } + + if (k.equals("dio1")) + { + c.dio1 = param.toInt(); + continue; + } + + if (k.equals("busy")) + { + c.busy = param.toInt(); + continue; + } + + if (k.equals("freq")) + { + c.clock_freq = param.toInt(); + continue; + } + + if (k.equals("msb")) + { + c.msb_first = !!param.toInt(); + continue; + } + + if (k.equals("spi_mode")) + { + c.spi_mode = param.toInt(); + continue; + } + + c.module = k; + c.cs = param.toInt(); + c.enabled = true; + } + + return c; +} + +String RadioModuleSPIConfig::toStr() +{ + if (!enabled) + { + return "none"; + } + + return module + ":" + String(cs) + ",bus:" + String(bus_num) + ",rst:" + String(rst) + + ",dio1:" + String(dio1) + ",busy:" + String(busy) + + ",freq:" + String(clock_freq) + ",msb:" + String(msb_first ? 1 : 0) + + ",spi_mode:" + String(spi_mode); +} + bool Config::write_config(const char *path) { File f = SD.open(path, FILE_WRITE, /*create = */ true); @@ -505,6 +635,12 @@ bool Config::write_config(const char *path) f.println("lora_enabled = " + getConfig("lora_enabled")); + f.println("uart0 = " + getConfig("uart0")); + f.println("uart1 = " + getConfig("uart1")); + f.println("spi1 = " + getConfig("spi1")); + f.println("wire1 = " + getConfig("wire1")); + f.println("radio2 = " + getConfig("radio2")); + f.close(); return true; } @@ -556,6 +692,31 @@ String Config::getConfig(String key) return String(is_host ? "true" : "false"); } + if (key.equalsIgnoreCase("uart0")) + { + return uart0.toStr(); + } + + if (key.equalsIgnoreCase("uart1")) + { + return uart1.toStr(); + } + + if (key.equalsIgnoreCase("spi1")) + { + return spi1.toStr(); + } + + if (key.equalsIgnoreCase("wire1")) + { + return wire1.toStr(); + } + + if (key.equalsIgnoreCase("radio2")) + { + return radio2.toStr(); + } + return ""; } @@ -626,3 +787,127 @@ ParseResult parse_config_line(String ln) return ParseResult(k, v); } + +BusConfig BusConfig::configure(String cfg) +{ + BusConfig c; + + if (cfg.equalsIgnoreCase("none")) + { + return c; + } + + int begin = 0; + int end, i; + + while ((i = findSepa(cfg, ",", begin, end)) >= 0) + { + String param = cfg.substring(begin, end); + begin = i; + int j = param.indexOf(":"); + if (j < 0) + { + Serial.printf("Expected ':' to be present in '%s' - ignoring config\n", + param); + continue; + } + + String k = param.substring(0, j); + param = param.substring(j + 1); + + k.toLowerCase(); + if (k.equals("enabled")) + { + c.enabled = !param.equalsIgnoreCase("false"); + continue; + } + + if (c.bus_type == SPI && k.equals("clk") || c.bus_type == WIRE && k.equals("scl")) + { + c.clk = param.toInt(); + continue; + } + + if (c.bus_type == SERIAL && k.equals("tx") || + c.bus_type == SPI && k.equals("mosi") || + c.bus_type == WIRE && k.equals("sda")) + { + c.tx = param.toInt(); + continue; + } + + if (c.bus_type == SERIAL && k.equals("rx") || + c.bus_type == SPI && k.equals("miso")) + { + c.rx = param.toInt(); + continue; + } + + if (c.bus_type != NONE) + { + Serial.printf("Unknown key: '%s'; ignoring config\n", k.c_str()); + continue; + } + + if (k.equals("none") || k.length() == 0) + { + continue; + } + + c.enabled = true; + + char bus_type = k.charAt(0); + switch (bus_type) + { + case 'u': + c.bus_type = UART; + break; + case 's': + c.bus_type = SPI; + break; + case 'w': + c.bus_type = WIRE; + break; + default: + c.bus_type = NONE; + c.enabled = false; + } + + c.bus_num = k.substring(1).toInt(); + c.clock_freq = param.toInt(); + } + + return c; +} + +String BusConfig::toStr() +{ + if (bus_type == NONE) + { + return "none"; + } + + String ret = String(bus_type == UART ? "u" + : bus_type == SPI ? "s" + : bus_type == WIRE ? "w" + : "none") + + String(bus_num); + + ret += ":" + String(clock_freq) + (enabled ? "" : ",enabled:false"); + switch (bus_type) + { + case UART: + ret += ",rx:" + String(rx) + ",tx:" + String(tx); + break; + case SPI: + ret += ",clk:" + String(clk) + ",mosi:" + String(mosi) + ",miso:" + String(miso); + break; + case WIRE: + ret += ",scl:" + String(scl) + ",sda:" + String(sda); + break; + default: + ret = "none"; + } + + return ret; +} diff --git a/lib/config/config.h b/lib/config/config.h index c09107b..4be175c 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -46,11 +46,102 @@ struct LoRaConfig uint16_t preamble_len; uint8_t sync_word; bool crc; + uint16_t crc_seed; + uint16_t crc_poly; uint8_t implicit_header; }; LoRaConfig *configureLora(String cfg); +struct BusConfig +{ + enum + { + NONE, // No bus + UART, // Serial + SPI, + WIRE // I2C + } bus_type; + + int8_t bus_num; + + bool enabled; + + uint32_t clock_freq; + + union + { + int8_t scl; + int8_t clk; + }; + + union + { + int8_t sda; + int8_t mosi; + int8_t tx; + }; + + union + { + int8_t miso; + int8_t rx; + }; + + BusConfig() + : bus_type(NONE), bus_num(-1), enabled(false), clock_freq(115200), clk(-1), + rx(-1), tx(-1) {}; + + static BusConfig configure(String cfg); + String toStr(); +}; + +#ifndef RADIO_MODULE_CS_PIN +#define RADIO_MODULE_CS_PIN 38 +#endif + +#ifndef RADIO_MODULE_DIO1_PIN +#define RADIO_MODULE_DIO1_PIN 40 +#endif + +#ifndef RADIO_MODULE_RST_PIN +#define RADIO_MODULE_RST_PIN 41 +#endif + +#ifndef RADIO_MODULE_BUSY_PIN +#define RADIO_MODULE_BUSY_PIN 39 +#endif + +#ifndef RADIO_MODULE_CLOCK_FREQ +#define RADIO_MODULE_CLOCK_FREQ 16000000 +#endif + +#ifndef RADIO_MODULE_MSB_FIRST +#define RADIO_MODULE_MSB_FIRST 1 +#endif + +#ifndef RADIO_MODULE_SPI_MODE +#define RADIO_MODULE_SPI_MODE 0 +#endif + +struct RadioModuleSPIConfig +{ + bool enabled; + String module; + int8_t bus_num; + int8_t cs; + int8_t rst; + int8_t dio1; + int8_t busy; + + uint32_t clock_freq; + bool msb_first; + int8_t spi_mode; + + static RadioModuleSPIConfig configure(String cfg); + String toStr(); +}; + #ifndef FREQ_RX #define FREQ_RX 866 #endif @@ -82,6 +173,26 @@ LoRaConfig *configureLora(String cfg); #define DEFAULT_LORA_SF 7 #endif +#ifndef DEFAULT_UART0 +#define DEFAULT_UART0 "none" +#endif + +#ifndef DEFAULT_UART1 +#define DEFAULT_UART1 "u1:115200" +#endif + +#ifndef DEFAULT_SPI1 +#define DEFAULT_SPI1 "s1:16000000,clk:42,mosi:46,miso:45" +#endif + +#ifndef DEFAULT_WIRE1 +#define DEFAULT_WIRE1 "none" +#endif + +#ifndef DEFAULT_RADIO2 +#define DEFAULT_RADIO2 "none" +#endif + #define CREATE_MISSING_CONFIG true struct Config { @@ -98,6 +209,12 @@ struct Config LoRaConfig *rx_lora; LoRaConfig *tx_lora; + BusConfig uart0; + BusConfig uart1; + BusConfig spi1; + BusConfig wire1; + RadioModuleSPIConfig radio2; + bool is_host; bool lora_enabled; @@ -108,7 +225,11 @@ struct Config listen_on_serial0(String("none")), listen_on_serial1(String("readline")), listen_on_usb(String("readline")), rx_lora(configureLora(DEFAULT_RX)), tx_lora(configureLora(DEFAULT_TX)), is_host(DEFAULT_IS_LORA_HOST), - lora_enabled(DEFAULT_LORA_ENABLED) {}; + uart0(BusConfig::configure(DEFAULT_UART0)), + uart1(BusConfig::configure(DEFAULT_UART1)), + spi1(BusConfig::configure(DEFAULT_SPI1)), + wire1(BusConfig::configure(DEFAULT_WIRE1)), lora_enabled(DEFAULT_LORA_ENABLED), + radio2(RadioModuleSPIConfig::configure(DEFAULT_RADIO2)) {}; bool write_config(const char *path); diff --git a/lib/events/events.h b/lib/events/events.h index 6f1dea0..b1b249b 100644 --- a/lib/events/events.h +++ b/lib/events/events.h @@ -18,6 +18,7 @@ struct Event struct { float rssi; + float rssi2; float freq; bool trigger; bool detected; diff --git a/lib/heading/Compass.cpp b/lib/heading/Compass.cpp new file mode 100644 index 0000000..8a86871 --- /dev/null +++ b/lib/heading/Compass.cpp @@ -0,0 +1,281 @@ +#include "heading.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; + + String err = selfTest(); + if (!err.startsWith("OK\n")) + { + return false; + } + + _lastErr = CompassStatus::COMPASS_OK; + return true; +} + +uint8_t _write_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t value, + bool skipValue = false) +{ + wire.beginTransmission(addr); + size_t s = wire.write(reg); + if (s == 1 && !skipValue) + { + s = wire.write(value); + } + + size_t s1 = wire.endTransmission(); + if (s != 1 && s1 == 0) + { + return 1; // "data too long to fit in transmit buffer" + } + + return s1; +} + +int8_t _read_registers(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t *v, size_t sz, + bool skipRegister = false) +{ + if (!skipRegister) + { + uint8_t s = _write_register(wire, addr, reg, 0, true); + if (s != 0) + { + return s; + } + } + + uint8_t r = wire.requestFrom(addr, sz); + for (int i = 0; i < r; i++, v++) + { + *v = wire.read(); + } + + return r - sz; +} + +uint8_t _read_register(TwoWire &wire, uint8_t addr, uint8_t reg, uint8_t &v, + bool skipRegister = false) +{ + uint8_t r = _read_registers(wire, addr, reg, &v, 1, skipRegister); + if (r != 0) + { + return 1; + } + + return 0; +} + +int8_t _read_xyz(TwoWire &wire, CompassXYZ &xyz) +{ + xyz.status = 0; + size_t s = _read_register(wire, QMC5883_ADDR, QMC5883_STATUS_REG, xyz.status); + if (s != 0) + { + return s; + } + + if ((xyz.status & QMC5883_STATUS_DRDY) == 0) + { + delay(10); + s = _read_register(wire, QMC5883_ADDR, QMC5883_STATUS_REG, xyz.status); + if (s != 0) + { + return s; + } + } + + int16_t mags[3]; + + int8_t r = _read_registers(wire, 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(wire, QMC5883_ADDR, QMC5883_FBR_REG, 0); + s |= _write_register(wire, 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(wire, QMC5883_ADDR, QMC5883_FBR_REG, 1); // set/reset period + if (s != 0) + { + return s; + } + + return _write_register(wire, 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(wire, 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(wire, 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..193faca --- /dev/null +++ b/lib/heading/heading.h @@ -0,0 +1,98 @@ +#pragma once +#include +#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 +{ + TwoWire &wire; + QMC5883LCompass(TwoWire &wire) : Compass(), wire(wire) {} + + 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/lib/loraboards/LiLyGo.h b/lib/loraboards/LiLyGo.h index 103240b..2be621b 100644 --- a/lib/loraboards/LiLyGo.h +++ b/lib/loraboards/LiLyGo.h @@ -18,10 +18,10 @@ #ifndef HELTEC_NO_RADIO_INSTANCE #ifndef ARDUINO_heltec_wifi_32_lora_V3 // Assume MISO and MOSI being wrong when not using Heltec's board definition -// and use hspi to make it work anyway. See heltec_setup() for the actual SPI setup. +// and use vspi to make it work anyway. See heltec_setup() for the actual SPI setup. #include -extern SPIClass *hspi; -#define RADIO_MODULE_INIT() new Module(SS, DIO1, RST_LoRa, BUSY_LoRa, *hspi); +extern SPIClass *vspi; +#define RADIO_MODULE_INIT() new Module(SS, DIO1, RST_LoRa, BUSY_LoRa, *vspi); #else // ARDUINO_heltec_wifi_32_lora_V3 #endif // end ARDUINO_heltec_wifi_32_lora_V3 #endif // end HELTEC_NO_RADIO_INSTANCE diff --git a/lib/loraboards/LoRaBoards.cpp b/lib/loraboards/LoRaBoards.cpp index 7353c99..61e515b 100644 --- a/lib/loraboards/LoRaBoards.cpp +++ b/lib/loraboards/LoRaBoards.cpp @@ -77,7 +77,7 @@ void heltec_setup() #ifdef HELTEC #ifndef ARDUINO_heltec_wifi_32_lora_V3 - hspi->begin(SCK, MISO, MOSI, SS); + vspi->begin(SCK, MISO, MOSI, SS); #endif #endif #ifndef HELTEC_NO_DISPLAY_INSTANCE @@ -90,7 +90,7 @@ void heltec_setup() #ifdef HELTEC #ifndef ARDUINO_heltec_wifi_32_lora_V3 -SPIClass hspi = new SPIClass(2); +SPIClass vspi = new SPIClass(2); #endif #endif @@ -533,7 +533,7 @@ bool beginDisplay() Wire.beginTransmission(DISPLAY_ADDR); if (Wire.endTransmission() == 0) { - Serial.printf("Find Display model at 0x%X address\n", DISPLAY_ADDR); + Serial.printf("Found Display model at 0x%X address\n", DISPLAY_ADDR); u8g2 = new DISPLAY_MODEL(U8G2_R0, U8X8_PIN_NONE); u8g2->begin(); u8g2->clearBuffer(); @@ -569,6 +569,7 @@ bool beginSDCard() else { Serial.println("Warning: Failed to init Sd Card"); + SDCardSPI.end(); } #endif return false; @@ -792,10 +793,9 @@ void setupBoards(bool disable_u8g2) bool sdReady; #ifndef DISABLE_SDCARD - for (int i = 0; i < 5 && !(sdReady = beginSDCard()); i++) + if (!(sdReady = beginSDCard())) { Serial.println("SD card failed or not found"); - delay(1000); } #endif diff --git a/lib/radio/SX1262.cpp b/lib/radio/SX1262.cpp new file mode 100644 index 0000000..fe2237b --- /dev/null +++ b/lib/radio/SX1262.cpp @@ -0,0 +1,49 @@ +#include "radio.h" +#include +#include + +SX1262Module::SX1262Module(RadioModuleSPIConfig radio2) : RadioModule() +{ + _radio = new SX1262(new Module( + radio2.cs, radio2.dio1, radio2.rst, radio2.busy, radio2.bus_num == 1 ? hspi : SPI, + SPISettings(radio2.clock_freq, radio2.msb_first ? MSBFIRST : LSBFIRST, + radio2.spi_mode))); + Serial.printf("Initialized Radio2: %s\n", radio2.toStr().c_str()); +} + +int16_t SX1262Module::beginScan(float init_freq, float bw, uint8_t shaping) +{ + int16_t status = _radio->beginFSK(init_freq); + if (status != RADIOLIB_ERR_NONE) + { + Serial.printf("Radio2: Failed beginFSK: %d\n", status); + return status; + } + + status = _radio->startReceive(RADIOLIB_SX126X_RX_TIMEOUT_NONE); + + if (status != RADIOLIB_ERR_NONE) + { + Serial.printf("Radio2: Failed startReceive: %d\n", status); + return status; + } + + status = setFrequency(init_freq); + if (status != RADIOLIB_ERR_NONE) + { + Serial.printf("Radio2: Failed setFrequency: %d\n", status); + return status; + } + + return status; +} + +int16_t SX1262Module::setFrequency(float freq) +{ + return _radio->setFrequency(freq, + true); // false = calibration is needed here +} + +int16_t SX1262Module::setRxBandwidth(float bw) { return _radio->setRxBandwidth(bw); } + +float SX1262Module::getRSSI() { return _radio->getRSSI(false); } diff --git a/lib/radio/radio.h b/lib/radio/radio.h new file mode 100644 index 0000000..81c14a1 --- /dev/null +++ b/lib/radio/radio.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +struct RadioModule +{ + RadioModule() {}; + + virtual int16_t beginScan(float init_freq, float bw, uint8_t shaping) = 0; + virtual int16_t setFrequency(float freq) = 0; + virtual int16_t setRxBandwidth(float bw) = 0; + virtual float getRSSI() = 0; +}; + +#ifdef USING_SX1262 +struct SX1262Module : RadioModule +{ + SX1262 *_radio; + + SX1262Module(RadioModuleSPIConfig cfg); + + int16_t beginScan(float init_freq, float bw, uint8_t shaping) override; + int16_t setFrequency(float freq) override; + int16_t setRxBandwidth(float bw) override; + + float getRSSI() override; +}; +#endif diff --git a/src/main.cpp b/src/main.cpp index a669bc0..efee144 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -262,6 +262,15 @@ void sendBTData(float heading, float rssi) #include #endif // end LILYGO +#include +#include +#include + +DroneHeading droneHeading; +Compass *compass = NULL; + +RadioModule *radio2; + #define BT_SCAN_DELAY 60 * 1 * 1000 #define WF_SCAN_DELAY 60 * 2 * 1000 long noDevicesMillis = 0, cycleCnt = 0; @@ -813,6 +822,7 @@ void osdProcess() Config config; +#ifdef USING_LR1121 void setLRFreq(float freq) { bool skipCalibration = false; @@ -838,6 +848,7 @@ void setLRFreq(float freq) radio.freqMHz = freq; radio.highFreq = (freq > 1000.0); } +#endif float getRSSI(void *param) { @@ -1057,6 +1068,25 @@ void init_radio() setFrequency(CONF_FREQ_BEGIN); delay(100); + + if (config.radio2.enabled && config.radio2.module.equalsIgnoreCase("SX1262")) + { + radio2 = new SX1262Module(config.radio2); + state = radio2->beginScan(CONF_FREQ_BEGIN, BANDWIDTH, RADIOLIB_SHAPING_NONE); + if (state == RADIOLIB_ERR_NONE) + { + both.println("Initialized additional module OK"); + radio2->setRxBandwidth(BANDWIDTH); + } + else + { + Serial.printf("Error initializing additional module: %d\n", state); + if (state == RADIOLIB_ERR_CHIP_NOT_FOUND) + { + Serial.println("Radio2: CHIP NOT FOUND"); + } + } + } } struct frequency_scan_result @@ -1088,12 +1118,19 @@ void eventListenerForReport(void *arg, Event &e) frequency_scan_result.readings_sz = frequency_scan_result.dump.sz + 1; uint32_t *f = new uint32_t[frequency_scan_result.readings_sz]; int16_t *r = new int16_t[frequency_scan_result.readings_sz]; + int16_t *r2 = radio2 ? new int16_t[frequency_scan_result.readings_sz] : NULL; if (old_sz > 0) { memcpy(f, frequency_scan_result.dump.freqs_khz, old_sz * sizeof(uint32_t)); memcpy(r, frequency_scan_result.dump.rssis, old_sz * sizeof(int16_t)); + if (radio2) + { + memcpy(r2, frequency_scan_result.dump.rssis2, + old_sz * sizeof(int16_t)); + delete[] frequency_scan_result.dump.rssis2; + } delete[] frequency_scan_result.dump.freqs_khz; delete[] frequency_scan_result.dump.rssis; @@ -1101,12 +1138,16 @@ void eventListenerForReport(void *arg, Event &e) frequency_scan_result.dump.freqs_khz = f; frequency_scan_result.dump.rssis = r; + frequency_scan_result.dump.rssis2 = r2; } frequency_scan_result.dump.freqs_khz[frequency_scan_result.dump.sz] = e.detected.freq * 1000; // convert to kHz frequency_scan_result.dump.rssis[frequency_scan_result.dump.sz] = max(e.detected.rssi, -999.0f); + if (radio2) + frequency_scan_result.dump.rssis2[frequency_scan_result.dump.sz] = + max(e.detected.rssi2, -999.0f); frequency_scan_result.dump.sz++; if (e.epoch != frequency_scan_result.last_epoch || @@ -1529,6 +1570,31 @@ void setup(void) wf_start = millis(); config = Config::init(); +#if defined(HAS_SDCARD) + SD.end(); + SDCardSPI.end(); // end SPI before other uses, eg radio2 over SPI +#endif + + pinMode(LED, OUTPUT); + pinMode(BUZZER_PIN, OUTPUT); + pinMode(REB_PIN, OUTPUT); + heltec_setup(); + + if (!initUARTs(config)) + { + Serial.println("Failed to initialize UARTs"); + } + + if (!initSPIs(config)) + { + Serial.println("Failed to initialize SPIs"); + } + + if (!initWires(config)) + { + Serial.println("Failed to initialize I2Cs"); + } + r.comms_initialized = Comms::initComms(config); if (r.comms_initialized) { @@ -1539,11 +1605,6 @@ void setup(void) Serial.println("Comms did not initialize"); } - pinMode(LED, OUTPUT); - pinMode(BUZZER_PIN, OUTPUT); - pinMode(REB_PIN, OUTPUT); - heltec_setup(); - #ifdef JOYSTICK_ENABLED calibrate_joy(); pinMode(JOY_BTN_PIN, INPUT_PULLUP); @@ -1769,6 +1830,29 @@ void setup(void) #endif + if (wireDevices & QMC5883L || wire1Devices & QMC5883L) + { + compass = new QMC5883LCompass(wireDevices & QMC5883L ? Wire : Wire1); + } + + if (compass) + { + 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 @@ -2149,6 +2233,9 @@ void sendMessage(RoutedMessage &m) #endif } break; + case HEADING: + droneHeading.setHeading(millis(), m.message->payload.heading.heading); + break; } } @@ -2259,6 +2346,7 @@ void doScan(); void reportScan(); long calStart = 0; +#ifdef COMPASS_ENABLED float getCompassHeading() { if (calStart == 0) @@ -2360,6 +2448,7 @@ float getCompassHeading() return headingDegrees; #endif } +#endif float historicalCompassRssi[STEPS] = {999}; int compassCounter = 0; @@ -2383,6 +2472,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(); @@ -2778,6 +2873,14 @@ void doScan() int display_x = x / SCAN_RBW_FACTOR; freqX[(int)r.current_frequency] = display_x; setFrequency(curr_freq / 1000.0); + if (radio2 != NULL) + { + state = radio2->setFrequency(curr_freq / 1000.0); + if (state != RADIOLIB_ERR_NONE) + { + Serial.printf("Radio2: Failed to set freq: %d\n", state); + } + } LOG("Step:%d Freq: %f\n", x, r.current_frequency); // SpectralScan Method @@ -2809,6 +2912,8 @@ void doScan() #endif #ifdef METHOD_RSSI // Spectrum analyzer using getRSSI + float rssi2 = -999; + { LOG("METHOD RSSI"); @@ -2828,6 +2933,7 @@ void doScan() else g = &getRSSI; uint16_t max_rssi = 120; + // Scan if not in the ignore list if (ignoredFreq.find((int)r.current_frequency) == ignoredFreq.end()) { @@ -2839,6 +2945,11 @@ void doScan() { xRSSI[display_x] = (int)max_rssi; } + + for (int i = 0; radio2 != NULL && i < samples; i++) + { + rssi2 = max(rssi2, radio2->getRSSI()); + } } else { @@ -2874,6 +2985,7 @@ void doScan() Event event = r.detect(result, filtered_result, RADIOLIB_SX126X_SPECTRAL_SCAN_RES_SIZE, samples); event.time_ms = millis(); + event.detected.rssi2 = rssi2; size_t detected_at = event.detected.detected_at; if (max_rssi_x > detected_at)