mirror of
https://github.com/Genaker/LoraSA.git
synced 2026-03-28 17:42:59 +01:00
460 lines
10 KiB
C++
460 lines
10 KiB
C++
#include "comms.h"
|
|
|
|
int packetRssi = 0;
|
|
|
|
int16_t RadioComms::configureRadio()
|
|
{
|
|
int16_t status =
|
|
radio.begin(loraCfg.freq, loraCfg.bw, loraCfg.sf, loraCfg.cr, loraCfg.sync_word,
|
|
loraCfg.tx_power, loraCfg.preamble_len);
|
|
if (status != RADIOLIB_ERR_NONE)
|
|
{
|
|
Serial.printf("could not configure Lora: %d\n", status);
|
|
return status;
|
|
}
|
|
|
|
if (!loraCfg.crc)
|
|
{
|
|
status = radio.setCRC(0);
|
|
if (status != RADIOLIB_ERR_NONE)
|
|
{
|
|
Serial.printf("could not configure CRC for Lora: %d\n", status);
|
|
return status;
|
|
}
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
size_t _write(uint8_t *m, size_t sz, size_t p, uint8_t v)
|
|
{
|
|
if (p >= sz)
|
|
return sz;
|
|
m[p] = v;
|
|
return p + 1;
|
|
}
|
|
|
|
size_t _write(uint8_t *m, size_t sz, size_t p, uint8_t *v, size_t v_sz)
|
|
{
|
|
while (v_sz > 0)
|
|
{
|
|
p = _write(m, sz, p, *v);
|
|
v++;
|
|
v_sz--;
|
|
}
|
|
|
|
return p;
|
|
}
|
|
|
|
#define MAX_MSG 128
|
|
#define RSSI_HI -80
|
|
#define DETAIL_RSSIS 8
|
|
#define RSSI_LO (RSSI_HI - 1 - DETAIL_RSSIS)
|
|
|
|
uint8_t *_serialize_scan_result(Message &m, size_t &p, uint8_t *msg)
|
|
{
|
|
if (m.type != SCAN_RESULT)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
size_t dump_sz = m.payload.dump.sz;
|
|
size_t max_msg = p;
|
|
p = _write(msg, max_msg, 0, (uint8_t)m.type);
|
|
|
|
// first cut: dump the RSSI as-is
|
|
// optimize the message size later
|
|
p = _write(msg, max_msg, p, (uint8_t *)&dump_sz, 2);
|
|
p = _write(msg, max_msg, p, (uint8_t *)&m.payload.dump.freqs_khz[0], 4);
|
|
p = _write(msg, max_msg, p, (uint8_t *)&m.payload.dump.freqs_khz[dump_sz - 1], 4);
|
|
|
|
size_t rem = max_msg - p;
|
|
if (rem > dump_sz)
|
|
rem = dump_sz;
|
|
|
|
uint8_t bits = 0;
|
|
size_t pp = p;
|
|
for (int i = 0; i < dump_sz; i++)
|
|
{
|
|
if (i * rem / dump_sz > p - pp)
|
|
{
|
|
p = _write(msg, max_msg, p, bits);
|
|
bits = 0;
|
|
}
|
|
int16_t v = m.payload.dump.rssis[i];
|
|
if (v >= 0)
|
|
{
|
|
v = 255;
|
|
}
|
|
else
|
|
{
|
|
v += 255;
|
|
if (v < 0)
|
|
{
|
|
v = 0;
|
|
}
|
|
}
|
|
|
|
bits = max(bits, (uint8_t)v);
|
|
}
|
|
|
|
if (dump_sz > 0)
|
|
{
|
|
p = _write(msg, max_msg, p, bits);
|
|
}
|
|
|
|
return msg;
|
|
}
|
|
|
|
uint8_t *_serialize_scan_max_result(Message &m, size_t &p, uint8_t *msg)
|
|
{
|
|
if (m.type != SCAN_MAX_RESULT)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
size_t dump_sz = m.payload.dump.sz;
|
|
size_t max_msg = p;
|
|
p = _write(msg, max_msg, 0, (uint8_t)m.type);
|
|
p = _write(msg, max_msg, p, (uint8_t *)&dump_sz, 2);
|
|
|
|
int16_t b = SCAN_MAX_RESULT_KHZ_SCALE; // scale to fit khz into 2 bytes
|
|
p = _write(msg, max_msg, p, (uint8_t)b);
|
|
|
|
for (int i = 0; i < dump_sz; i++)
|
|
{
|
|
b = m.payload.dump.freqs_khz[i] / SCAN_MAX_RESULT_KHZ_SCALE;
|
|
p = _write(msg, max_msg, p, (uint8_t *)&b, 2);
|
|
b = m.payload.dump.rssis[i];
|
|
if (b >= 0)
|
|
{
|
|
b = 255;
|
|
}
|
|
else
|
|
{
|
|
b += 255;
|
|
if (b < 0)
|
|
{
|
|
b = 0;
|
|
}
|
|
}
|
|
p = _write(msg, max_msg, p, (uint8_t)b);
|
|
}
|
|
|
|
return msg;
|
|
}
|
|
|
|
uint8_t *_serialize_config_task(Message &m, size_t &p, uint8_t *msg)
|
|
{
|
|
if (m.type != CONFIG_TASK)
|
|
{
|
|
return NULL;
|
|
}
|
|
size_t max_msg = p;
|
|
ConfigTaskType ctt = m.payload.config.task_type;
|
|
p = _write(msg, max_msg, 0, (uint8_t)m.type);
|
|
p = _write(msg, max_msg, p, (uint8_t)ctt);
|
|
|
|
int key_len = m.payload.config.key->length();
|
|
if (max_msg - p < key_len + 1)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
p = _write(msg, max_msg, p, (uint8_t)key_len);
|
|
p = _write(msg, max_msg, p, (uint8_t *)m.payload.config.key->c_str(), key_len);
|
|
|
|
if (ctt == GET || ctt == SET_FAIL)
|
|
{
|
|
return msg;
|
|
}
|
|
|
|
int v_len = m.payload.config.value->length();
|
|
|
|
if (max_msg - p < v_len + 1)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
p = _write(msg, max_msg, p, (uint8_t)v_len);
|
|
p = _write(msg, max_msg, p, (uint8_t *)m.payload.config.value->c_str(), v_len);
|
|
|
|
return msg;
|
|
}
|
|
|
|
int16_t RadioComms::send(Message &m)
|
|
{
|
|
uint8_t msg_buf[MAX_MSG];
|
|
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);
|
|
}
|
|
else if (m.type == MessageType::SCAN_MAX_RESULT)
|
|
{
|
|
msg = _serialize_scan_max_result(m, p, msg_buf);
|
|
}
|
|
else if (m.type == MessageType::CONFIG_TASK)
|
|
{
|
|
msg = _serialize_config_task(m, p, msg_buf);
|
|
}
|
|
|
|
if (msg == NULL)
|
|
{
|
|
Serial.printf("Failed to encode message\n");
|
|
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)
|
|
{
|
|
delete[] msg;
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
size_t _read(uint8_t *buf, size_t sz, size_t p, uint8_t *v, size_t len)
|
|
{
|
|
for (; p < sz && len > 0; v++, p++, len--)
|
|
{
|
|
*v = buf[p];
|
|
}
|
|
|
|
return p;
|
|
}
|
|
|
|
size_t _read(uint8_t *buf, size_t sz, size_t p, uint8_t *v)
|
|
{
|
|
return _read(buf, sz, p, v, 1);
|
|
}
|
|
|
|
Message *_deserialize_scan_result(size_t len, size_t &p, uint8_t *packet)
|
|
{
|
|
Message *message = new Message();
|
|
message->type = SCAN_RESULT;
|
|
|
|
uint32_t s, e;
|
|
size_t dump_sz = 0;
|
|
p = _read(packet, len, p, (uint8_t *)&dump_sz, 2);
|
|
p = _read(packet, len, p, (uint8_t *)&s, 4);
|
|
p = _read(packet, len, p, (uint8_t *)&e, 4);
|
|
size_t rem = len - p;
|
|
|
|
message->payload.dump.sz = dump_sz;
|
|
message->payload.dump.prssi = packetRssi;
|
|
if (dump_sz > 0)
|
|
{
|
|
message->payload.dump.rssis = new int16_t[dump_sz];
|
|
message->payload.dump.freqs_khz = new uint32_t[dump_sz];
|
|
message->payload.dump.freqs_khz[0] = s;
|
|
message->payload.dump.freqs_khz[dump_sz - 1] = e;
|
|
|
|
for (int i = 1; i < dump_sz - 1; i++)
|
|
{
|
|
uint32_t incr = (e - s) / (dump_sz - i);
|
|
s += incr;
|
|
message->payload.dump.freqs_khz[i] = s;
|
|
}
|
|
|
|
for (int i = 0, k = 0; i < rem; i++)
|
|
{
|
|
int j = (i + 1) * dump_sz / rem;
|
|
int16_t rssi = 0;
|
|
p = _read(packet, len, p, (uint8_t *)&rssi);
|
|
rssi -= 255;
|
|
for (; k < j; k++)
|
|
{
|
|
message->payload.dump.rssis[k] = rssi;
|
|
}
|
|
}
|
|
}
|
|
|
|
return message;
|
|
}
|
|
|
|
Message *_deserialize_scan_max_result(size_t len, size_t &p, uint8_t *packet)
|
|
{
|
|
Message *message = new Message();
|
|
message->type = SCAN_MAX_RESULT;
|
|
|
|
uint32_t b = 0;
|
|
size_t dump_sz = 0;
|
|
p = _read(packet, len, p, (uint8_t *)&dump_sz, 2);
|
|
uint32_t *freqs = new uint32_t[dump_sz];
|
|
int16_t *rssis = new int16_t[dump_sz];
|
|
message->payload.dump.sz = dump_sz;
|
|
message->payload.dump.freqs_khz = freqs;
|
|
message->payload.dump.rssis = rssis;
|
|
|
|
uint32_t scale = 0;
|
|
p = _read(packet, len, p, (uint8_t *)&scale);
|
|
|
|
for (int i = 0; i < dump_sz; i++)
|
|
{
|
|
p = _read(packet, len, p, (uint8_t *)&b, 2);
|
|
freqs[i] = scale * b;
|
|
|
|
b = 0;
|
|
p = _read(packet, len, p, (uint8_t *)&b);
|
|
rssis[i] = ((int16_t)b) - 255;
|
|
}
|
|
|
|
return message;
|
|
}
|
|
|
|
Message *_deserialize_config_task(size_t len, size_t &p, uint8_t *packet)
|
|
{
|
|
Message *message = new Message();
|
|
message->type = CONFIG_TASK;
|
|
|
|
ConfigTaskType ctt = GET;
|
|
p = _read(packet, len, p, (uint8_t *)&ctt);
|
|
message->payload.config.task_type = ctt;
|
|
|
|
size_t key_len = 0;
|
|
size_t p1 = _read(packet, len, p, (uint8_t *)&key_len);
|
|
memmove(packet + p, packet + p + 1, key_len);
|
|
packet[p + key_len] = 0;
|
|
String *key = new String((char *)packet + p);
|
|
message->payload.config.key = key;
|
|
|
|
p = p1 + key_len;
|
|
|
|
if (key->length() != key_len)
|
|
{
|
|
delete message;
|
|
return NULL;
|
|
}
|
|
|
|
if (ctt == GET || ctt == SET_FAIL)
|
|
{
|
|
return message;
|
|
}
|
|
|
|
size_t v_len = 0;
|
|
p1 = _read(packet, len, p, (uint8_t *)&v_len);
|
|
memmove(packet + p, packet + p + 1, v_len);
|
|
packet[p + v_len] = 0;
|
|
String *value = new String((char *)packet + p);
|
|
message->payload.config.value = value;
|
|
|
|
p = p1 + v_len;
|
|
|
|
if (value->length() != v_len)
|
|
{
|
|
delete message;
|
|
return NULL;
|
|
}
|
|
|
|
return message;
|
|
}
|
|
|
|
volatile bool _received = false;
|
|
void _rcv() { _received = true; }
|
|
|
|
Message *RadioComms::receive(uint16_t timeout_ms)
|
|
{
|
|
uint8_t msg[MAX_MSG];
|
|
|
|
#ifdef USING_LR1121
|
|
Message *message = NULL;
|
|
#warning Radio Comms not fully supported for LR1121
|
|
#else
|
|
// because of this, receive is single-threaded, single-device
|
|
_received = false;
|
|
radio.setDio1Action(_rcv);
|
|
uint32_t timeout_ticks = (uint32_t)timeout_ms * (1000000 / 15625);
|
|
|
|
int16_t status = radio.startReceive(timeout_ticks);
|
|
if (status != RADIOLIB_ERR_NONE)
|
|
{
|
|
radio.clearDio1Action();
|
|
Serial.printf("Failed to start receive: %d\n", status);
|
|
return NULL;
|
|
}
|
|
|
|
// wait on a semaphore
|
|
while (!_received)
|
|
{
|
|
yield();
|
|
}
|
|
radio.clearDio1Action();
|
|
|
|
packetRssi = radio.getRSSI(true);
|
|
// Serial.println("LORA_RSSI:" + String(packetRssi));
|
|
size_t len = radio.getPacketLength(true);
|
|
uint8_t *packet = msg;
|
|
|
|
if (len > MAX_MSG)
|
|
{
|
|
packet = new uint8_t[len];
|
|
}
|
|
status = radio.readData(packet, len);
|
|
|
|
if (status == RADIOLIB_ERR_RX_TIMEOUT)
|
|
{
|
|
return NULL;
|
|
}
|
|
|
|
if (status != RADIOLIB_ERR_NONE || len == 0)
|
|
{
|
|
if (packet != msg)
|
|
delete[] packet;
|
|
|
|
Serial.printf("Failed to read data: E(%d), len == %d\n", status, len);
|
|
return NULL;
|
|
}
|
|
|
|
if (len > MAX_MSG)
|
|
{
|
|
Serial.printf("Packet is longer than expected: %d\n", len);
|
|
}
|
|
|
|
size_t p;
|
|
uint8_t b;
|
|
p = _read(packet, len, 0, &b);
|
|
|
|
Message *message = NULL;
|
|
if (b == SCAN_RESULT)
|
|
{
|
|
message = _deserialize_scan_result(len, p, packet);
|
|
}
|
|
else if (b == SCAN_MAX_RESULT)
|
|
{
|
|
message = _deserialize_scan_max_result(len, p, packet);
|
|
}
|
|
else if (b == CONFIG_TASK)
|
|
{
|
|
message = _deserialize_config_task(len, p, packet);
|
|
}
|
|
else
|
|
{
|
|
Serial.printf("Received message type %" PRIu8 ", length %d, but expecting %" PRIu8
|
|
" - ignoring\n",
|
|
b, len, (uint8_t)MessageType::SCAN_RESULT);
|
|
}
|
|
|
|
if (packet != msg)
|
|
{
|
|
delete[] packet;
|
|
}
|
|
#endif
|
|
return message;
|
|
}
|