From 726a5e797f7da273627964dd94efdc837450d177 Mon Sep 17 00:00:00 2001 From: Sassa NF Date: Sun, 13 Apr 2025 14:16:45 +0100 Subject: [PATCH] Send heading + scan result --- lib/comms/comms.cpp | 6 ++++- lib/comms/comms.h | 3 +++ lib/comms/radio_comms.cpp | 53 +++++++++++++++++++++++++++++++++++++++ src/main.cpp | 38 +++++++++++++++++++++++++++- 4 files changed, 98 insertions(+), 2 deletions(-) diff --git a/lib/comms/comms.cpp b/lib/comms/comms.cpp index c0215d0..ec5b066 100644 --- a/lib/comms/comms.cpp +++ b/lib/comms/comms.cpp @@ -432,12 +432,16 @@ String _wrap_str(String v) Message::~Message() { - if (type == SCAN_RESULT || type == SCAN_MAX_RESULT) + if (type == SCAN_RESULT || type == SCAN_MAX_RESULT || type == SCAN_HEADING_MAX) { if (payload.dump.sz > 0) { delete[] payload.dump.freqs_khz; delete[] payload.dump.rssis; + if (payload.dump.rssis2) + { + delete[] payload.dump.rssis2; + } payload.dump.sz = 0; } diff --git a/lib/comms/comms.h b/lib/comms/comms.h index 57fb90b..68370f6 100644 --- a/lib/comms/comms.h +++ b/lib/comms/comms.h @@ -20,6 +20,7 @@ enum MessageType SCAN, SCAN_RESULT, SCAN_MAX_RESULT, + SCAN_HEADING_MAX, CONFIG_TASK, HEADING, _MAX_MESSAGE_TYPE = HEADING @@ -53,6 +54,8 @@ struct ScanTaskResult int16_t *rssis; int16_t *rssis2; int16_t prssi; + int16_t heading_min; + int16_t heading_max; }; struct ConfigTask diff --git a/lib/comms/radio_comms.cpp b/lib/comms/radio_comms.cpp index a994cdb..026588d 100644 --- a/lib/comms/radio_comms.cpp +++ b/lib/comms/radio_comms.cpp @@ -144,6 +144,32 @@ uint8_t *_serialize_scan_max_result(Message &m, size_t &p, uint8_t *msg) return msg; } +uint8_t *_serialize_scan_heading_max(Message &m, size_t &p, uint8_t *msg) +{ + if (m.type != SCAN_HEADING_MAX) + { + return NULL; + } + + size_t written = 0; + written = _write(msg, p, 0, (uint8_t *)&m.payload.dump.heading_min, 2); + written = _write(msg, p, written, (uint8_t *)&m.payload.dump.heading_max, 2); + + uint8_t *sub_msg = msg + written; + p -= written; + m.type = SCAN_MAX_RESULT; + sub_msg = _serialize_scan_max_result(m, p, sub_msg); + p += written; + m.type = SCAN_HEADING_MAX; + + if (sub_msg == NULL) + { + msg = NULL; + } + + return msg; +} + uint8_t *_serialize_config_task(Message &m, size_t &p, uint8_t *msg) { if (m.type != CONFIG_TASK) @@ -201,6 +227,10 @@ int16_t RadioComms::send(Message &m) { msg = _serialize_scan_max_result(m, p, msg_buf); } + else if (m.type == MessageType::SCAN_HEADING_MAX) + { + msg = _serialize_scan_heading_max(m, p, msg_buf); + } else if (m.type == MessageType::CONFIG_TASK) { msg = _serialize_config_task(m, p, msg_buf); @@ -301,6 +331,7 @@ Message *_deserialize_scan_max_result(size_t len, size_t &p, uint8_t *packet) message->payload.dump.sz = dump_sz; message->payload.dump.freqs_khz = freqs; message->payload.dump.rssis = rssis; + message->payload.dump.rssis2 = NULL; uint32_t scale = 0; p = _read(packet, len, p, (uint8_t *)&scale); @@ -318,6 +349,24 @@ Message *_deserialize_scan_max_result(size_t len, size_t &p, uint8_t *packet) return message; } +Message *_deserialize_scan_heading_max(size_t len, size_t &p, uint8_t *packet) +{ + int16_t heading_min = -999; + int16_t heading_max = 0; + p = _read(packet, len, p, (uint8_t *)&heading_min, 2); + p = _read(packet, len, p, (uint8_t *)&heading_max, 2); + + Message *message = _deserialize_scan_max_result(len, p, packet); + + if (message != NULL) + { + message->type = SCAN_HEADING_MAX; + message->payload.dump.heading_min = heading_min; + message->payload.dump.heading_max = heading_max; + } + return message; +} + Message *_deserialize_config_task(size_t len, size_t &p, uint8_t *packet) { Message *message = new Message(); @@ -439,6 +488,10 @@ Message *RadioComms::receive(uint16_t timeout_ms) { message = _deserialize_scan_max_result(len, p, packet); } + else if (b == SCAN_HEADING_MAX) + { + message = _deserialize_scan_heading_max(len, p, packet); + } else if (b == CONFIG_TASK) { message = _deserialize_config_task(len, p, packet); diff --git a/src/main.cpp b/src/main.cpp index 396e18b..5e825b6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1149,6 +1149,8 @@ void eventListenerForReport(void *arg, Event &e) if (e.epoch != frequency_scan_result.last_epoch) { frequency_scan_result.dump.sz = 0; + frequency_scan_result.dump.heading_min = -999; + frequency_scan_result.dump.heading_max = -999; } if (frequency_scan_result.dump.sz >= frequency_scan_result.readings_sz) @@ -1196,6 +1198,29 @@ void eventListenerForReport(void *arg, Event &e) frequency_scan_result.rssi = e.detected.rssi; } + HeadingSensor *sensor = &droneHeading; + if (compass != NULL && compass->lastRead() > sensor->lastRead()) + { + sensor = compass; + } + + if (sensor->lastRead() > -1) + { + int16_t heading = sensor->heading(); + if (frequency_scan_result.dump.heading_min == -999) + { + frequency_scan_result.dump.heading_min = heading; + frequency_scan_result.dump.heading_max = heading; + } + else + { + frequency_scan_result.dump.heading_min = + min(frequency_scan_result.dump.heading_min, heading); + frequency_scan_result.dump.heading_max = + min(frequency_scan_result.dump.heading_max, heading); + } + } + return; } @@ -2072,7 +2097,8 @@ void routeMessage(RoutedMessage &m) } if (m.message->type == MessageType::SCAN_RESULT || - m.message->type == MessageType::SCAN_MAX_RESULT) + m.message->type == MessageType::SCAN_MAX_RESULT || + m.message->type == MessageType::SCAN_HEADING_MAX) { m.to.host = 1; return; @@ -2266,6 +2292,7 @@ void sendMessage(RoutedMessage &m) break; case SCAN_RESULT: case SCAN_MAX_RESULT: + case SCAN_HEADING_MAX: if (config.is_host) { #ifdef DISPLAY_RAW_SCAN @@ -3411,6 +3438,8 @@ void reportScan() Message m; m.type = SCAN_RESULT; m.payload.dump.sz = 0; + m.payload.dump.rssis2 = NULL; + m.payload.dump.heading_min = -999; if (config.detection_strategy.equalsIgnoreCase("RSSI")) { @@ -3428,6 +3457,13 @@ void reportScan() { m.type = SCAN_MAX_RESULT; + m.payload.dump.heading_min = frequency_scan_result.dump.heading_min; + m.payload.dump.heading_max = frequency_scan_result.dump.heading_max; + if (m.payload.dump.heading_min > -999) + { + m.type = SCAN_HEADING_MAX; + } + size_t sz = config.scan_ranges_sz; m.payload.dump.sz = sz; m.payload.dump.freqs_khz = new uint32_t[sz];