From 3d7ab81f6f0ea4dff991a0cd7466c3dd85988158 Mon Sep 17 00:00:00 2001 From: Sassa NF Date: Tue, 31 Dec 2024 11:01:34 +0000 Subject: [PATCH] Rework radio message processing --- lib/comms/comms.cpp | 17 ++- lib/comms/comms.h | 11 +- lib/comms/radio_comms.cpp | 224 ++++++++++++++++++++++++++++++-------- lib/config/config.h | 10 ++ src/main.cpp | 126 +++++++++++++++------ 5 files changed, 298 insertions(+), 90 deletions(-) diff --git a/lib/comms/comms.cpp b/lib/comms/comms.cpp index 53781b3..b68bcde 100644 --- a/lib/comms/comms.cpp +++ b/lib/comms/comms.cpp @@ -265,12 +265,18 @@ bool ReadlineComms::send(Message &m) p = _scan_result_str(m.payload.dump); break; case MessageType::CONFIG_TASK: - p = m.payload.config.is_set ? "SET " : "GET "; + ConfigTaskType ctt = m.payload.config.task_type; + p = ctt == GET ? "GET " + : ctt == SET ? "SET " + : ctt == GETSET_SUCCESS ? "Success: " + : "Failed to set: "; p += *m.payload.config.key; - if (m.payload.config.is_set) + if (ctt == SET || ctt == GETSET_SUCCESS) { p += " " + *m.payload.config.value; } + + p += "\n"; break; } @@ -368,7 +374,7 @@ Message *_parsePacket(String p) { Message *m = new Message(); m->type = MessageType::CONFIG_TASK; - m->payload.config.is_set = false; + m->payload.config.task_type = GET; m->payload.config.key = new String(_stringParam(p, "")); m->payload.config.value = NULL; return m; @@ -378,7 +384,7 @@ Message *_parsePacket(String p) { Message *m = new Message(); m->type = MessageType::CONFIG_TASK; - m->payload.config.is_set = true; + m->payload.config.task_type = SET; m->payload.config.key = new String(_stringParam(p, "")); m->payload.config.value = new String(_stringParam(p, "")); @@ -430,7 +436,8 @@ Message::~Message() { delete payload.config.key; - if (payload.config.is_set) + ConfigTaskType ctt = payload.config.task_type; + if (ctt == GETSET_SUCCESS || ctt == SET) { delete payload.config.value; } diff --git a/lib/comms/comms.h b/lib/comms/comms.h index b852a06..990babb 100644 --- a/lib/comms/comms.h +++ b/lib/comms/comms.h @@ -22,6 +22,15 @@ enum MessageType _MAX_MESSAGE_TYPE = CONFIG_TASK }; +enum ConfigTaskType +{ + GET = 0, + SET, + GETSET_SUCCESS, + SET_FAIL, + _MAX_CONFIG_TASK_TYPE = SET_FAIL +}; + struct Wrapper { int32_t length; @@ -45,7 +54,7 @@ struct ConfigTask { String *key; String *value; - bool is_set; + ConfigTaskType task_type; }; struct Message diff --git a/lib/comms/radio_comms.cpp b/lib/comms/radio_comms.cpp index fa269b6..0cce77b 100644 --- a/lib/comms/radio_comms.cpp +++ b/lib/comms/radio_comms.cpp @@ -48,24 +48,25 @@ size_t _write(uint8_t *m, size_t sz, size_t p, uint8_t *v, size_t v_sz) #define RSSI_HI -80 #define DETAIL_RSSIS 8 #define RSSI_LO (RSSI_HI - 1 - DETAIL_RSSIS) -int16_t RadioComms::send(Message &m) + +uint8_t *_serialize_scan_result(Message &m, size_t &p, uint8_t *msg) { if (m.type != SCAN_RESULT) { - return RADIOLIB_ERR_INVALID_FUNCTION; + return NULL; } - uint8_t msg[MAX_MSG]; size_t dump_sz = m.payload.dump.sz; - size_t p = _write(msg, MAX_MSG, 0, (uint8_t)m.type); + 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 *)&m.payload.dump.freqs_khz[0], 4); - p = _write(msg, MAX_MSG, p, (uint8_t *)&m.payload.dump.freqs_khz[dump_sz - 1], 4); - 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); + p = _write(msg, max_msg, p, (uint8_t *)&dump_sz, 2); - size_t rem = MAX_MSG - p; + size_t rem = max_msg - p; if (rem > dump_sz) rem = dump_sz; @@ -75,7 +76,7 @@ int16_t RadioComms::send(Message &m) { if (i * rem / dump_sz > p - pp) { - p = _write(msg, MAX_MSG, p, bits); + p = _write(msg, max_msg, p, bits); bits = 0; } int16_t v = m.payload.dump.rssis[i]; @@ -97,10 +98,79 @@ int16_t RadioComms::send(Message &m) if (dump_sz > 0) { - p = _write(msg, MAX_MSG, p, bits); + p = _write(msg, max_msg, p, bits); } - return radio.transmit(msg, p); + 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 (m.type == SCAN_RESULT) + { + msg = _serialize_scan_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; + } + + 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) @@ -118,6 +188,96 @@ 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 *)&s, 4); + p = _read(packet, len, p, (uint8_t *)&e, 4); + p = _read(packet, len, p, (uint8_t *)&dump_sz, 2); + size_t rem = len - p; + + message->payload.dump.sz = dump_sz; + 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_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; } @@ -184,43 +344,11 @@ Message *RadioComms::receive(uint16_t timeout_ms) Message *message = NULL; if (b == SCAN_RESULT) { - message = new Message(); - message->type = SCAN_RESULT; - - uint32_t s, e; - size_t dump_sz = 0; - p = _read(packet, len, p, (uint8_t *)&s, 4); - p = _read(packet, len, p, (uint8_t *)&e, 4); - p = _read(packet, len, p, (uint8_t *)&dump_sz, 2); - size_t rem = len - p; - - message->payload.dump.sz = dump_sz; - 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; - } - } - } + message = _deserialize_scan_result(len, p, packet); + } + else if (b == CONFIG_TASK) + { + message = _deserialize_config_task(len, p, packet); } else { diff --git a/lib/config/config.h b/lib/config/config.h index 29ce03e..8c9fc59 100644 --- a/lib/config/config.h +++ b/lib/config/config.h @@ -3,6 +3,16 @@ #include +template struct Result +{ + bool is_ok; + union + { + L not_ok; + R ok; + }; +}; + struct ScanRange { uint64_t start_khz; diff --git a/src/main.cpp b/src/main.cpp index 6b24e25..b693133 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1425,7 +1425,13 @@ void checkComms() Comms1->send(*m); // forward to peer break; case MessageType::CONFIG_TASK: - if (m->payload.config.is_set) + ConfigTaskType ctt = m->payload.config.task_type; + if (ctt == GET) + { + Serial.printf("GET config: %s = %s\n", m->payload.config.key->c_str(), + config.getConfig(*m->payload.config.key).c_str()); + } + else if (ctt == SET) { String v = config.getConfig(*m->payload.config.key); bool r = @@ -1494,7 +1500,8 @@ void doScan(); void reportScan(RadioComms &c); -int16_t checkRadio(RadioComms &c); +Result checkRadio(RadioComms &c); +int16_t sendMessage(RadioComms &c, Message &m); void loop(void) { @@ -1504,27 +1511,43 @@ void loop(void) drone_detected_frequency_start = 0; checkComms(); + // NB: swapping the use of Tx and Rx comms, so a pair of modules + // with identical rx/tx_lora config can talk + RadioComms *rx = config.is_host ? TxComms : RxComms; + RadioComms *tx = config.is_host ? RxComms : TxComms; - if (config.is_host) + if (rx != NULL && (config.is_host || config.lora_enabled)) { - if (TxComms != NULL) + Result res = checkRadio(*rx); + + if (!res.is_ok) { - // NB: swapping the use of Tx and Rx comms, so a pair of modules - // with identical rx/tx_lora config can talk - int16_t status = checkRadio(*TxComms); + int16_t status = res.not_ok; if (status != RADIOLIB_ERR_NONE) { Serial.printf("Error getting a message: %d\n", status); } } + else if (res.ok != NULL) + { + if (config.is_host || tx == NULL) + { + HostComms->send(*res.ok); + } + else + { + sendMessage(*tx, *res.ok); + } + + delete res.ok; + } } - else + + if (!config.is_host) { doScan(); - if (TxComms != NULL && config.lora_enabled) - reportScan(*TxComms); - if (RxComms != NULL && config.lora_enabled) - checkRadio(*RxComms); + if (tx != NULL && config.lora_enabled) + reportScan(*tx); } } @@ -1961,56 +1984,87 @@ void doScan() #endif } -int16_t checkRadio(RadioComms &comms) +Result checkRadio(RadioComms &comms) { radioIsScan = false; - int16_t status = comms.configureRadio(); - if (status != RADIOLIB_ERR_NONE) - return status; + Result ret; + ret.is_ok = false; + + ret.not_ok = comms.configureRadio(); + if (ret.not_ok != RADIOLIB_ERR_NONE) + return ret; + + ret.is_ok = true; Message *msg = comms.receive( config.is_host ? 2000 : 200); // 200ms should be enough to receive 500 bytes at SF 7 and BW 500 + ret.ok = msg; if (msg == NULL) { - return status; + return ret; } - if (msg->type == SCAN_RESULT) + if (msg->type == CONFIG_TASK) { - HostComms->send(*msg); - } - else - { - Serial.printf("Received a message of unsupported type: %d\n", msg->type); + ConfigTaskType ctt = msg->payload.config.task_type; + + if (ctt == ConfigTaskType::GET || ctt == ConfigTaskType::SET) + { + // must be GET or SET - both require sending back a response + + String old_v = config.getConfig(*msg->payload.config.key); + bool success = true; + if (ctt == ConfigTaskType::SET) + { + success = config.updateConfig(*msg->payload.config.key, + *msg->payload.config.value); + delete msg->payload.config.value; + } + + if (success) + { + msg->payload.config.task_type = GETSET_SUCCESS; + msg->payload.config.value = new String(old_v); + } + else + { + msg->payload.config.task_type = SET_FAIL; + } + } } - delete msg; - - return status; + return ret; } -void reportScan(RadioComms &comms) +int16_t sendMessage(RadioComms &comms, Message &msg) { radioIsScan = false; int16_t status = comms.configureRadio(); if (status != RADIOLIB_ERR_NONE) { Serial.printf("Failed to configure Radio: %d\n", status); - return; + return status; } - Message m; - m.type = SCAN_RESULT; - m.payload.dump = frequency_scan_result.dump; - status = comms.send(m); - - m.payload.dump.sz = 0; // dump is shared, so should not delete underlying arrays + status = comms.send(msg); if (status != RADIOLIB_ERR_NONE) { - Serial.printf("Failed to send scan result: %d\n", status); - return; + Serial.printf("Failed to send message of type %d: %d\n", msg.type, status); + return status; } + + return status; +} + +void reportScan(RadioComms &comms) +{ + Message m; + m.type = SCAN_RESULT; + m.payload.dump = frequency_scan_result.dump; + int16_t status = sendMessage(comms, m); + + m.payload.dump.sz = 0; // dump is shared, so should not delete underlying arrays }