iarv/LoraSA
1
0
Fork 0
mirror of https://github.com/Genaker/LoraSA.git synced 2026-03-28 17:42:59 +01:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #134 from Genaker/testable

Browse Source 
Fixup compass compilation, add CRC to LoRa
This commit is contained in:
Yegor Shytikov
2025-01-27 20:46:35 -08:00
committed by GitHub
parent e6865febda 42dcab94ce
commit c7727fb527
9 changed files with 498 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
lib/comms/comms.cpp
View File

@@ -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)

33
lib/comms/comms.h
View File

@@ -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;

14
lib/comms/radio_comms.cpp
View File

@@ -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;

18
lib/config/config.cpp
View File

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

2
lib/config/config.h
View File

@@ -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;
};

284
lib/heading/Compass.cpp Normal file
View File

@@ -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;
}

10
lib/heading/DroneHeading.cpp Normal file
View File

@@ -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; }

96
lib/heading/heading.h Normal file
View File

@@ -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

34
src/main.cpp
View File

@@ -139,6 +139,11 @@ void sendBTData(float heading, float rssi)
#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;
@@ -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();