Pass spectrum to BLE

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

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

src/main.cpp

@@ -62,9 +62,9 @@
 #include <scan.h>
 #include <stdlib.h>
 
-#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 <heltec_unofficial.h>
@@ -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:

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