diff --git a/lib/heading/DroneHeading.cpp b/lib/heading/DroneHeading.cpp index 475184b..8d631a4 100644 --- a/lib/heading/DroneHeading.cpp +++ b/lib/heading/DroneHeading.cpp @@ -8,3 +8,34 @@ void DroneHeading::setHeading(int64_t now, int16_t h) int64_t DroneHeading::lastRead() { return _lastRead; } int16_t DroneHeading::heading() { return _heading; } + +int16_t meanHeading(int16_t h1, int16_t h2) +{ + while (h1 < 0) + { + h1 += 360; + } + + while (h2 < 0) + { + h2 += 360; + } + + h1 = h1 % 360; + h2 = h2 % 360; + + if (h1 > h2) + { + h1 += h2; + h2 = h1 - h2; + h1 -= h2; + } + // h1 is min, h2 is max + + if (h2 - h1 > 180) + { + h1 += 360; + } + + return (h1 + h2) / 2; +} diff --git a/lib/heading/heading.h b/lib/heading/heading.h index 6ce058f..4afee0f 100644 --- a/lib/heading/heading.h +++ b/lib/heading/heading.h @@ -119,6 +119,8 @@ struct DroneHeading : HeadingSensor int16_t heading() override; }; +int16_t meanHeading(int16_t m, int16_t mm); + #define QMC5883_ADDR 0xD #define QMC5883_X_LSB_REG 0 #define QMC5883_X_MSB_REG 1 diff --git a/src/main.cpp b/src/main.cpp index fe3be48..9330205 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -62,9 +62,9 @@ #include #include -#ifdef BT_MOBILE +bool bleDeviceConnected = false; -bool deviceConnected = false; +#ifdef BT_MOBILE #define SERVICE_UUID "00001234-0000-1000-8000-00805f9b34fb" #define CHARACTERISTIC_UUID "00001234-0000-1000-8000-00805f9b34ac" @@ -80,11 +80,11 @@ BLEAdvertising *pAdvertising = NULL; class MyServerCallbacks : public BLEServerCallbacks { - void onConnect(BLEServer *pServer) { deviceConnected = true; }; + void onConnect(BLEServer *pServer) { bleDeviceConnected = true; }; void onDisconnect(BLEServer *pServer) { - deviceConnected = false; + bleDeviceConnected = false; BLEDevice::startAdvertising(); // Restart advertising after disconnect } }; @@ -101,7 +101,7 @@ class BTServerCallbacks : public NimBLEServerCallbacks public: void onConnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo) override { - deviceConnected = true; + bleDeviceConnected = true; Serial.printf("Device Connected | Free Heap: %d kByte\n", ESP.getFreeHeap() / 1000); Serial.printf("Client address: %s\n", connInfo.getAddress().toString().c_str()); @@ -112,7 +112,7 @@ class BTServerCallbacks : public NimBLEServerCallbacks void onDisconnect(NimBLEServer *pServer, NimBLEConnInfo &connInfo, int reason) override { - deviceConnected = false; + bleDeviceConnected = false; Serial.println("Device Disconnected"); NimBLEDevice::startAdvertising(); // Restart advertising } @@ -265,6 +265,8 @@ void initBT() #endif } +#endif + // Function to send RSSI and Heading Data void sendBTData(float heading, float rssi) { @@ -274,10 +276,45 @@ void sendBTData(float heading, float rssi) #ifdef COMPASS_DEBUG Serial.println("Sending data: " + data); #endif +#ifdef BT_MOBILE pCharacteristic->setValue(data.c_str()); // Set BLE characteristic value pCharacteristic->notify(); // Notify connected client -} #endif +} + +// Send Scan Result to BLE +void sendBTData(Message &msg) +{ + if (msg.type != SCAN_HEADING_MAX && msg.type != SCAN_MAX_RESULT && + msg.type != SCAN_RESULT) + { + Serial.println("Unsupported message type: " + String(msg.type)); + return; + } + + String data = "{\"SCAN_RESULT\":{\"Hmin\":" + String(msg.payload.dump.heading_min) + + ",\"Hmax\":" + String(msg.payload.dump.heading_max) + ",\"Spectrum\":["; + + for (int i = 0; i < msg.payload.dump.sz; i++) + { + data += String(i == 0 ? "" : ",") + + "{\"F\":" + String(msg.payload.dump.freqs_khz[i]) + + ",\"R\":" + String(msg.payload.dump.rssis[i]) + + (msg.payload.dump.rssis2 == NULL + ? "" + : ",\"R2\":" + String(msg.payload.dump.rssis2[i])) + + "}"; + } + + data += "]}}"; +#ifdef COMPASS_DEBUG + Serial.println("Sending data: " + data); +#endif +#ifdef BT_MOBILE + pCharacteristic->setValue(data.c_str()); // Set BLE characteristic value + pCharacteristic->notify(); // Notify connected client +#endif +} #ifndef LILYGO #include @@ -2248,13 +2285,19 @@ void sendMessage(RoutedMessage &m) { if (msg->type == SCAN_HEADING_MAX) { - droneHeading.setHeading(millis(), msg->payload.heading.heading); + droneHeading.setHeading(millis(), + meanHeading(msg->payload.dump.heading_min, + msg->payload.dump.heading_max)); } #ifdef DISPLAY_RAW_SCAN display_raw_scan(m.message->payload.dump); #else display_scan_result(m.message->payload.dump); #endif + if (bleDeviceConnected) + { + sendBTData(*msg); + } } break; case HEADING: diff --git a/web_app/compass/index.html b/web_app/compass/index.html index 4f16b4d..8228948 100644 --- a/web_app/compass/index.html +++ b/web_app/compass/index.html @@ -336,8 +336,32 @@ const heading = parseInt(match[1]); const rssi = parseFloat(match[2]); console.log("H:" + heading + " R:" + rssi); - dataPoints[parseInt(heading)] = { angle: parseInt(heading), rssi: rssi }; - currentPoint = { angle: parseInt(heading), rssi: rssi }; + data = '{"SCAN_RESULT":{"Hmin":' + match[1] + ',"Hmax":' + match[1] + ',"Spectrum":[{"F":0,"R":' + match[2] + '}]}}'; + } + + try { + data = JSON.parse(data); + } catch (e) { + console.log("Skipping broken JSON:", e, "in", data); + return; + } + + if (data["SCAN_RESULT"]) { + scanResult = data["SCAN_RESULT"]; + const spectrum = scanResult["Spectrum"]; + if (spectrum.length == 0) { + console.log("Skipping scan result with no spectrum:", data); + return; + } + + const headingMin = scanResult.Hmin; + const headingMax = scanResult.Hmax; + + const heading = ((headingMax + headingMin + 720 + (headingMax - headingMin > 180 ? 360 : 0)) / 2) % 360; + const rssi = spectrum[0]["R"]; + + dataPoints[heading] = { angle: heading, rssi: rssi }; + currentPoint = { angle: heading, rssi: rssi }; //if (dataPoints.length > 50) dataPoints.shift(); // Keep only the last 50 points headingDisplay.textContent = `${heading.toFixed(1)}°`; rssiDisplay.textContent = `${rssi.toFixed(1)} dBm`;