BLE Rx Improvements (#2710)

* Work to allow for unique beacon parsing functions.
* Fix Copyright
* Update firmware/application/apps/ble_rx_app.cpp
* Update firmware/baseband/proc_btlerx.cpp
* PR suggestions.
* Fix String.
* Refactor

firmware/application/apps/ble_rx_app.cpp

@@ -57,6 +57,95 @@ std::string pad_string_with_spaces(int snakes) {
     return paddedStr;
 }
 
+struct GainEntry {
+    uint8_t lna;
+    uint8_t vga;
+    uint8_t gain;
+};
+
+// Only LNA with VGA 0-4 is tested to be accurate. Max zeroized gain tested to be 16dBm.
+// Beyond that it is hard to tell distance to transmitting device.
+// Test was conducted within a few inches of the device.
+// Device was transmitting at 0dBm.
+constexpr GainEntry gain_table[] =
+    {
+        {40, 0, 19},
+        {32, 0, 18},
+        {24, 0, 15},
+        {16, 0, 8},
+        {8, 0, 2},
+        {0, 0, 0},
+        {40, 2, 20},
+        {32, 2, 22},
+        {24, 2, 14},
+        {16, 2, 8},
+        {8, 2, 2},
+        {0, 2, 0},
+        {40, 4, 21},
+        {32, 4, 22},
+        {24, 4, 15},
+        {16, 4, 10},
+        {8, 4, 3},
+        {0, 4, 0},
+        {40, 6, 26},
+        {32, 6, 22},
+        {24, 6, 15},
+        {16, 6, 10},
+        {8, 6, 4},
+        {0, 6, 0},
+        {40, 8, 26},
+        {32, 8, 26},
+        {24, 8, 18},
+        {16, 8, 12},
+        {8, 8, 6},
+        {0, 8, 1},
+        {40, 10, 26},
+        {32, 10, 26},
+        {24, 10, 20},
+        {16, 10, 15},
+        {8, 10, 8},
+        {0, 10, 3},
+        {40, 12, 26},
+        {32, 12, 26},
+        {24, 12, 23},
+        {16, 12, 17},
+        {8, 12, 10},
+        {0, 12, 4},
+        {40, 14, 26},
+        {32, 14, 26},
+        {24, 14, 25},
+        {16, 14, 19},
+        {8, 14, 12},
+        {0, 14, 6},
+        {40, 16, 26},
+        {32, 16, 26},
+        {24, 16, 26},
+        {16, 16, 20},
+        {8, 16, 13},
+        {0, 16, 7},
+        {40, 18, 26},
+        {32, 18, 26},
+        {24, 18, 26},
+        {16, 18, 21},
+        {8, 18, 14},
+        {0, 18, 8},
+        {40, 20, 26},
+        {32, 20, 26},
+        {24, 20, 26},
+        {16, 20, 23},
+        {8, 20, 16},
+        {0, 20, 10},
+};
+
+uint8_t get_total_gain(uint8_t lna, uint8_t vga) {
+    for (const auto& entry : gain_table) {
+        if (entry.lna == lna && entry.vga == vga)
+            return entry.gain;
+    }
+
+    return 0;
+}
+
 uint64_t copy_mac_address_to_uint64(const uint8_t* macAddress) {
     uint64_t result = 0;
 
@@ -68,22 +157,6 @@ uint64_t copy_mac_address_to_uint64(const uint8_t* macAddress) {
     return result;
 }
 
-void reverse_byte_array(uint8_t* arr, int length) {
-    int start = 0;
-    int end = length - 1;
-
-    while (start < end) {
-        // Swap elements at start and end
-        uint8_t temp = arr[start];
-        arr[start] = arr[end];
-        arr[end] = temp;
-
-        // Move the indices towards the center
-        start++;
-        end--;
-    }
-}
-
 MAC_VENDOR_STATUS lookup_mac_vendor_status(const uint8_t* mac_address, std::string& vendor_name) {
     static bool db_checked = false;
     static bool db_exists = false;
@@ -128,6 +201,22 @@ std::string lookup_mac_vendor(const uint8_t* mac_address) {
     return vendor_name;
 }
 
+void reverse_byte_array(uint8_t* arr, int length) {
+    int start = 0;
+    int end = length - 1;
+
+    while (start < end) {
+        // Swap elements at start and end
+        uint8_t temp = arr[start];
+        arr[start] = arr[end];
+        arr[end] = temp;
+
+        // Move the indices towards the center
+        start++;
+        end--;
+    }
+}
+
 namespace ui {
 
 std::string pdu_type_to_string(ADV_PDU_TYPE type) {
@@ -186,19 +275,26 @@ void RecentEntriesTable<BleRecentEntries>::draw(
     if (!entry.nameString.empty() && entry.include_name) {
         line = entry.nameString;
 
-        if (line.length() < 17) {
+        if (line.length() < 10) {
-            line += pad_string_with_spaces(17 - line.length());
+            line += pad_string_with_spaces(10 - line.length());
         } else {
-            line = truncate(line, 17);
+            line = truncate(line, 10);
         }
     } else {
         line = to_string_mac_address(entry.packetData.macAddress, 6, false);
     }
 
+    std::string hitsStr;
+
+    if (!entry.informationString.empty()) {
+        hitsStr = entry.informationString;
+    } else {
+        hitsStr = "Hits: " + to_string_dec_int(entry.numHits);
+    }
+
     // Pushing single digit values down right justified.
-    std::string hitsStr = to_string_dec_int(entry.numHits);
     int hitsDigits = hitsStr.length();
-    uint8_t hits_spacing = 8 - hitsDigits;
+    uint8_t hits_spacing = 14 - hitsDigits;
 
     // Pushing single digit values down right justified.
     std::string dbStr = to_string_dec_int(entry.dbValue);
@@ -539,7 +635,6 @@ BLERxView::BLERxView(NavigationView& nav)
     logger = std::make_unique<BLELogger>();
 
     check_log.on_select = [this](Checkbox&, bool v) {
-        str_log = "";
         logging = v;
 
         if (logger && logging)
@@ -561,6 +656,7 @@ BLERxView::BLERxView(NavigationView& nav)
 
     button_clear_list.on_select = [this](Button&) {
         recent.clear();
+        recent_entries_view.set_dirty();
     };
 
     button_switch.on_select = [&nav](Button&) {
@@ -602,7 +698,10 @@ BLERxView::BLERxView(NavigationView& nav)
 
     options_filter.on_change = [this](size_t index, int32_t v) {
         filter_index = (uint8_t)index;
+        recent.clear();
         handle_filter_options(v);
+        uniqueParsing = filter_index == 2 ? true : false;
+        recent_entries_view.set_dirty();
     };
 
     options_channel.set_selected_index(channel_index, true);
@@ -775,49 +874,25 @@ bool BLERxView::saveFile(const std::filesystem::path& path) {
 }
 
 void BLERxView::on_data(BlePacketData* packet) {
-    if (!logging) {
-        str_log = "";
-    }
-
-    str_console += pdu_type_to_string((ADV_PDU_TYPE)packet->type);
-    str_console += " Len:";
-    str_console += to_string_dec_uint(packet->size);
-    str_console += " Mac:";
-    str_console += to_string_mac_address(packet->macAddress, 6, false);
-    str_console += " Data:";
-
-    int i;
-
-    for (i = 0; i < packet->dataLen; i++) {
-        str_console += to_string_hex(packet->data[i], 2);
-    }
-
     uint64_t macAddressEncoded = copy_mac_address_to_uint64(packet->macAddress);
 
     // Start of Packet stuffing.
     // Masking off the top 2 bytes to avoid invalid keys.
+
+    BleRecentEntry tempEntry;
+
+    if (updateEntry(packet, tempEntry, (ADV_PDU_TYPE)packet->type)) {
         auto& entry = ::on_packet(recent, macAddressEncoded & 0xFFFFFFFFFFFF);
-    updateEntry(packet, entry, (ADV_PDU_TYPE)packet->type);
 
-    // Add entries if they meet the criteria.
+        // Preserve exisisting data from entry.
-    // auto value = filter;
+        tempEntry.macAddress = macAddressEncoded;
-    // resetFilteredEntries(recent, [&value](const BleRecentEntry& entry) {
+        tempEntry.numHits = ++entry.numHits;
-    //     return (entry.dataString.find(value) == std::string::npos) && (entry.nameString.find(value) == std::string::npos);
+
-    // });
+        entry = tempEntry;
+
         handle_filter_options(options_filter.selected_index());
-
         handle_entries_sort(options_sort.selected_index());
 
-    // Log at End of Packet.
-    if (logger && logging) {
-        logger->log_raw_data(str_console + "\r\n");
-    }
-
-    if (serial_logging) {
-        UsbSerialAsyncmsg::asyncmsg(str_console);  // new line handled there, no need here.
-    }
-    str_console = "";
-
         if (!searchList.empty()) {
             auto it = searchList.begin();
 
@@ -835,6 +910,34 @@ void BLERxView::on_data(BlePacketData* packet) {
 
             text_found_count.set(to_string_dec_uint(found_count) + "/" + to_string_dec_uint(total_count));
         }
+    }
+
+    log_ble_packet(packet);
+}
+
+void BLERxView::log_ble_packet(BlePacketData* packet) {
+    str_console = "";
+    str_console += pdu_type_to_string((ADV_PDU_TYPE)packet->type);
+    str_console += " Len:";
+    str_console += to_string_dec_uint(packet->size);
+    str_console += " Mac:";
+    str_console += to_string_mac_address(packet->macAddress, 6, false);
+    str_console += " Data:";
+
+    int i;
+
+    for (i = 0; i < packet->dataLen; i++) {
+        str_console += to_string_hex(packet->data[i], 2);
+    }
+
+    // Log at End of Packet.
+    if (logger && logging) {
+        logger->log_raw_data(str_console);
+    }
+
+    if (serial_logging) {
+        UsbSerialAsyncmsg::asyncmsg(str_console);  // new line handled there, no need here.
+    }
 }
 
 void BLERxView::on_filter_change(std::string value) {
@@ -979,16 +1082,18 @@ BLERxView::~BLERxView() {
     baseband::shutdown();
 }
 
-void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry, ADV_PDU_TYPE pdu_type) {
+bool BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry, ADV_PDU_TYPE pdu_type) {
     std::string data_string;
 
+    bool success = false;
+
     int i;
 
     for (i = 0; i < packet->dataLen; i++) {
         data_string += to_string_hex(packet->data[i], 2);
     }
 
-    entry.dbValue = packet->max_dB;
+    entry.dbValue = 2 * (packet->max_dB - get_total_gain(receiver_model.lna(), receiver_model.vga()));
     entry.timestamp = to_string_timestamp(rtc_time::now());
     entry.dataString = data_string;
 
@@ -1004,7 +1109,6 @@ void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry,
     entry.packetData.macAddress[4] = packet->macAddress[4];
     entry.packetData.macAddress[5] = packet->macAddress[5];
 
-    entry.numHits++;
     entry.pduType = pdu_type;
     entry.channelNumber = channel_number;
 
@@ -1021,35 +1125,48 @@ void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry,
     entry.include_name = check_name.value();
 
     // Only parse name for advertisment packets and empty name entries
-    if ((pdu_type == ADV_IND || pdu_type == ADV_NONCONN_IND || pdu_type == SCAN_RSP || pdu_type == ADV_SCAN_IND) && entry.nameString.empty()) {
+    if (pdu_type == ADV_IND || pdu_type == ADV_NONCONN_IND)  // || pdu_type == SCAN_RSP || pdu_type == ADV_SCAN_IND)
-        ADV_PDU_PAYLOAD_TYPE_0_2_4_6* advertiseData = (ADV_PDU_PAYLOAD_TYPE_0_2_4_6*)entry.packetData.data;
+    {
+        if (uniqueParsing) {
+            // Add your unique beacon parsing function here.
+        }
 
-        uint8_t currentByte = 0;
+        if (!success && !uniqueParsing) {
-        uint8_t length = 0;
+            success = parse_beacon_data(packet->data, packet->dataLen, entry.nameString, entry.informationString);
-        uint8_t type = 0;
+        }
 
-        std::string decoded_data;
-        for (currentByte = 0; (currentByte < entry.packetData.dataLen);) {
-            length = advertiseData->Data[currentByte++];
-            type = advertiseData->Data[currentByte++];
-
-            // Subtract 1 because type is part of the length.
-            for (int i = 0; i < length - 1; i++) {
-                // parse the name of bluetooth device: 0x08->Shortened Local Name; 0x09->Complete Local Name
-                if (type == 0x08 || type == 0x09) {
-                    decoded_data += (char)advertiseData->Data[currentByte];
-                }
-                currentByte++;
-            }
-            if (!decoded_data.empty()) {
-                entry.nameString = std::move(decoded_data);
-                break;
-            }
-        }
     } else if (pdu_type == ADV_DIRECT_IND || pdu_type == SCAN_REQ) {
         ADV_PDU_PAYLOAD_TYPE_1_3* directed_mac_data = (ADV_PDU_PAYLOAD_TYPE_1_3*)entry.packetData.data;
         reverse_byte_array(directed_mac_data->A1, 6);
     }
+
+    return success;
+}
+
+bool BLERxView::parse_beacon_data(const uint8_t* data, uint8_t length, std::string& nameString, std::string& informationString) {
+    uint8_t currentByte, currentLength, currentType = 0;
+
+    for (currentByte = 0; currentByte < length;) {
+        currentLength = data[currentByte++];
+        currentType = data[currentByte++];
+
+        // Subtract 1 because type is part of the length.
+        for (int i = 0; ((i < currentLength - 1) && (currentByte < length)); i++) {
+            // parse the name of bluetooth device: 0x08->Shortened Local Name; 0x09->Complete Local Name
+            if (currentType == 0x08 || currentType == 0x09) {
+                nameString += (char)data[currentByte];
+            }
+            currentByte++;
+        }
+    }
+
+    if (nameString.empty()) {
+        nameString = "None";
+    }
+
+    informationString = "";
+
+    return true;
 }
 
 } /* namespace ui */

firmware/application/apps/ble_rx_app.hpp

@@ -84,7 +84,7 @@ typedef enum {
 struct BleRecentEntry {
     using Key = uint64_t;
 
-    static constexpr Key invalid_key = 0xffffffff;
+    static constexpr Key invalid_key = 0xFFFFFFFFFFFF;
 
     uint64_t macAddress;
     int dbValue;
@@ -92,6 +92,7 @@ struct BleRecentEntry {
     std::string timestamp;
     std::string dataString;
     std::string nameString;
+    std::string informationString;
     bool include_name;
     uint16_t numHits;
     ADV_PDU_TYPE pduType;
@@ -111,6 +112,7 @@ struct BleRecentEntry {
           timestamp{},
           dataString{},
           nameString{},
+          informationString{},
           include_name{},
           numHits{},
           pduType{},
@@ -216,15 +218,18 @@ class BLERxView : public View {
     bool saveFile(const std::filesystem::path& path);
     std::unique_ptr<UsbSerialThread> usb_serial_thread{};
     void on_data(BlePacketData* packetData);
+    void log_ble_packet(BlePacketData* packet);
     void on_filter_change(std::string value);
     void on_file_changed(const std::filesystem::path& new_file_path);
     void file_error();
     void on_timer();
     void handle_entries_sort(uint8_t index);
     void handle_filter_options(uint8_t index);
-    void updateEntry(const BlePacketData* packet, BleRecentEntry& entry, ADV_PDU_TYPE pdu_type);
+    bool updateEntry(const BlePacketData* packet, BleRecentEntry& entry, ADV_PDU_TYPE pdu_type);
+    bool parse_beacon_data(const uint8_t* data, uint8_t length, std::string& nameString, std::string& informationString);
 
     NavigationView& nav_;
+
     RxRadioState radio_state_{
         2402000000 /* frequency */,
         4000000 /* bandwidth */,
@@ -234,6 +239,7 @@ class BLERxView : public View {
     uint8_t channel_index{0};
     uint8_t sort_index{0};
     uint8_t filter_index{0};
+    bool uniqueParsing = false;
     std::string filter{};
     bool logging{false};
     bool serial_logging{false};
@@ -325,9 +331,10 @@ class BLERxView : public View {
 
     OptionsField options_filter{
         {18 * 8 + 2, 2 * 8},
-        4,
+        7,
         {{"Data", 0},
-         {"MAC", 1}}};
+         {"MAC", 1},
+         {"Unique", 2}}};
 
     Checkbox check_log{
         {10 * 8, 4 * 8 + 2},
@@ -380,9 +387,9 @@ class BLERxView : public View {
     BleRecentEntries tempList{};
 
     const RecentEntriesColumns columns{{
-        {"Mac Address", 17},
+        {"Name", 10},
-        {"Hits", 7},
+        {"Information", 13},
-        {"dB", 4},
+        {"dBm", 4},
     }};
 
     BleRecentEntriesView recent_entries_view{columns, recent};

firmware/application/log_file.cpp

@@ -28,10 +28,10 @@ Optional<File::Error> LogFile::write_entry(const std::string& entry) {
 
 Optional<File::Error> LogFile::write_entry(const rtc::RTC& datetime, const std::string& entry) {
     std::string timestamp = to_string_timestamp(datetime);
-    return write_line(timestamp + " " + entry);
+    return write_raw(timestamp + " " + entry);
 }
 
-Optional<File::Error> LogFile::write_line(const std::string& message) {
+Optional<File::Error> LogFile::write_raw(const std::string& message) {
     auto error = file.write_line(message);
     if (!error) {
         file.sync();

firmware/application/log_file.hpp

@@ -39,11 +39,10 @@ class LogFile {
 
     Optional<File::Error> write_entry(const std::string& entry);
     Optional<File::Error> write_entry(const rtc::RTC& datetime, const std::string& entry);
+    Optional<File::Error> write_raw(const std::string& message);
 
    private:
     File file{};
-
-    Optional<File::Error> write_line(const std::string& message);
 };
 
 #endif /*__LOG_FILE_H__*/

firmware/baseband/proc_btlerx.cpp

@@ -27,6 +27,15 @@
 
 #include "event_m4.hpp"
 
+float BTLERxProcessor::get_phase_diff(const complex16_t& sample0, const complex16_t& sample1) {
+    // Calculate the phase difference between two samples.
+    float dI = sample1.real() * sample0.real() + sample1.imag() * sample0.imag();
+    float dQ = sample1.imag() * sample0.real() - sample1.real() * sample0.imag();
+    float phase_diff = atan2f(dQ, dI);
+
+    return phase_diff;
+}
+
 uint32_t BTLERxProcessor::crc_init_reorder(uint32_t crc_init) {
     int i;
     uint32_t crc_init_tmp, crc_init_input, crc_init_input_tmp;
@@ -122,120 +131,111 @@ int BTLERxProcessor::verify_payload_byte(int num_payload_byte, ADV_PDU_TYPE pdu_
     return 0;
 }
 
+void BTLERxProcessor::resetOffsetTracking() {
+    frequency_offset = 0.0f;
+    frequency_offset_estimate = 0.0f;
+    phase_buffer_index = 0;
+    memset(phase_buffer, 0, sizeof(phase_buffer));
+}
+
+void BTLERxProcessor::resetBitPacketIndex() {
+    memset(rb_buf, 0, sizeof(rb_buf));
+    packet_index = 0;
+    bit_index = 0;
+}
+
+void BTLERxProcessor::resetToDefaultState() {
+    parseState = Parse_State_Begin;
+    resetOffsetTracking();
+    resetBitPacketIndex();
+}
+
+void BTLERxProcessor::demodulateFSKBits(int num_demod_byte) {
+    for (; packet_index < num_demod_byte; packet_index++) {
+        for (; bit_index < 8; bit_index++) {
+            if (samples_eaten >= (int)dst_buffer.count) {
+                return;
+            }
+
+            float phaseSum = 0.0f;
+            for (int k = 0; k < SAMPLE_PER_SYMBOL; ++k) {
+                float phase = get_phase_diff(
+                    dst_buffer.p[samples_eaten + k],
+                    dst_buffer.p[samples_eaten + k + 1]);
+                phaseSum += phase;
+            }
+
+            // phaseSum /= (SAMPLE_PER_SYMBOL);
+            // phaseSum -= frequency_offset;
+
+            bool bitDecision = (phaseSum > 0.0f);
+            rb_buf[packet_index] = rb_buf[packet_index] | (bitDecision << bit_index);
+
+            samples_eaten += SAMPLE_PER_SYMBOL;
+        }
+
+        bit_index = 0;
+    }
+}
+
 void BTLERxProcessor::handleBeginState() {
-    int num_symbol_left = dst_buffer.count / SAMPLE_PER_SYMBOL;  // One buffer sample consist of I and Q.
+    uint32_t validAccessAddress = DEFAULT_ACCESS_ADDR;
-
-    static uint8_t demod_buf_access[SAMPLE_PER_SYMBOL][LEN_DEMOD_BUF_ACCESS];
-
-    uint32_t uint32_tmp = DEFAULT_ACCESS_ADDR;
-    uint8_t accessAddrBits[LEN_DEMOD_BUF_ACCESS];
-
     uint32_t accesssAddress = 0;
 
-    // Filling up addressBits with the access address we are looking to find.
-    for (int i = 0; i < 32; i++) {
-        accessAddrBits[i] = 0x01 & uint32_tmp;
-        uint32_tmp = (uint32_tmp >> 1);
-    }
-
-    const int demod_buf_len = LEN_DEMOD_BUF_ACCESS;  // For AA
-    int demod_buf_offset = 0;
     int hit_idx = (-1);
-    bool unequal_flag = false;
 
-    memset(demod_buf_access, 0, SAMPLE_PER_SYMBOL * demod_buf_len);
+    for (int i = samples_eaten; i < (int)dst_buffer.count; i += SAMPLE_PER_SYMBOL) {
-
+        float phaseDiff = 0;
-    for (int i = 0; i < num_symbol_left * SAMPLE_PER_SYMBOL; i += SAMPLE_PER_SYMBOL) {
-        int sp = ((demod_buf_offset - demod_buf_len + 1) & (demod_buf_len - 1));
 
         for (int j = 0; j < SAMPLE_PER_SYMBOL; j++) {
-            // Sample and compare with the adjacent next sample.
+            phaseDiff += get_phase_diff(dst_buffer.p[i + j], dst_buffer.p[i + j + 1]);
-            int I0 = dst_buffer.p[i + j].real();
+        }
-            int Q0 = dst_buffer.p[i + j].imag();
-            int I1 = dst_buffer.p[i + j + 1].real();
-            int Q1 = dst_buffer.p[i + j + 1].imag();
 
-            int phase_idx = j;
+        phase_buffer[phase_buffer_index] = phaseDiff / (SAMPLE_PER_SYMBOL);
+        phase_buffer_index = (phase_buffer_index + 1) % ROLLING_WINDOW;
 
-            demod_buf_access[phase_idx][demod_buf_offset] = (I0 * Q1 - I1 * Q0) > 0 ? 1 : 0;
+        bool bitDecision = (phaseDiff > 0);
 
-            int k = sp;
+        accesssAddress = (accesssAddress >> 1 | (bitDecision << 31));
-            unequal_flag = false;
 
-            accesssAddress = 0;
+        int errors = __builtin_popcount(accesssAddress ^ validAccessAddress) & 0xFFFFFFFF;
+
+        if (errors <= 4) {
+            hit_idx = i + SAMPLE_PER_SYMBOL;
+
+            for (int k = 0; k < ROLLING_WINDOW; k++) {
+                frequency_offset_estimate += phase_buffer[k];
+            }
+
+            frequency_offset = frequency_offset_estimate / ROLLING_WINDOW;
 
-            for (int p = 0; p < demod_buf_len; p++) {
-                if (demod_buf_access[phase_idx][k] != accessAddrBits[p]) {
-                    unequal_flag = true;
-                    hit_idx = (-1);
             break;
         }
-
-                accesssAddress = (accesssAddress & (~(1 << p))) | (demod_buf_access[phase_idx][k] << p);
-
-                k = ((k + 1) & (demod_buf_len - 1));
-            }
-
-            if (unequal_flag == false) {
-                hit_idx = (i + j - (demod_buf_len - 1) * SAMPLE_PER_SYMBOL);
-                break;
-            }
-        }
-
-        if (unequal_flag == false) {
-            break;
-        }
-
-        demod_buf_offset = ((demod_buf_offset + 1) & (demod_buf_len - 1));
     }
 
     if (hit_idx == -1) {
         // Process more samples.
+        samples_eaten = dst_buffer.count + 1;
         return;
     }
 
-    symbols_eaten += hit_idx;
+    samples_eaten += hit_idx;
-
-    symbols_eaten += (8 * NUM_ACCESS_ADDR_BYTE * SAMPLE_PER_SYMBOL);  // move to the beginning of PDU header
-
-    num_symbol_left = num_symbol_left - symbols_eaten;
 
     parseState = Parse_State_PDU_Header;
 }
 
 void BTLERxProcessor::handlePDUHeaderState() {
-    int num_demod_byte = 2;  // PDU header has 2 octets
+    if (samples_eaten > (int)dst_buffer.count) {
-
-    symbols_eaten += 8 * num_demod_byte * SAMPLE_PER_SYMBOL;
-
-    if (symbols_eaten > (int)dst_buffer.count) {
         return;
     }
 
-    // Jump back down to the beginning of PDU header.
+    demodulateFSKBits(NUM_PDU_HEADER_BYTE);
-    sample_idx = symbols_eaten - (8 * num_demod_byte * SAMPLE_PER_SYMBOL);
 
-    packet_index = 0;
+    if (packet_index < NUM_PDU_HEADER_BYTE || bit_index != 0) {
-
+        return;
-    for (int i = 0; i < num_demod_byte; i++) {
-        rb_buf[packet_index] = 0;
-
-        for (int j = 0; j < 8; j++) {
-            int I0 = dst_buffer.p[sample_idx].real();
-            int Q0 = dst_buffer.p[sample_idx].imag();
-            int I1 = dst_buffer.p[sample_idx + 1].real();
-            int Q1 = dst_buffer.p[sample_idx + 1].imag();
-
-            bit_decision = (I0 * Q1 - I1 * Q0) > 0 ? 1 : 0;
-            rb_buf[packet_index] = rb_buf[packet_index] | (bit_decision << j);
-
-            sample_idx += SAMPLE_PER_SYMBOL;
     }
 
-        packet_index++;
+    scramble_byte(rb_buf, 2, scramble_table[channel_number], rb_buf);
-    }
-
-    scramble_byte(rb_buf, num_demod_byte, scramble_table[channel_number], rb_buf);
 
     pdu_type = (ADV_PDU_TYPE)(rb_buf[0] & 0x0F);
     // uint8_t tx_add = ((rb_buf[0] & 0x40) != 0);
@@ -252,30 +252,16 @@ void BTLERxProcessor::handlePDUHeaderState() {
 }
 
 void BTLERxProcessor::handlePDUPayloadState() {
-    int i;
+    const int num_demod_byte = (payload_len + 3);
-    int num_demod_byte = (payload_len + 3);
-    symbols_eaten += 8 * num_demod_byte * SAMPLE_PER_SYMBOL;
 
-    if (symbols_eaten > (int)dst_buffer.count) {
+    if (samples_eaten > (int)dst_buffer.count) {
         return;
     }
 
-    for (i = 0; i < num_demod_byte; i++) {
+    demodulateFSKBits(num_demod_byte + NUM_PDU_HEADER_BYTE);
-        rb_buf[packet_index] = 0;
 
-        for (int j = 0; j < 8; j++) {
+    if (packet_index < (num_demod_byte + NUM_PDU_HEADER_BYTE) || bit_index != 0) {
-            int I0 = dst_buffer.p[sample_idx].real();
+        return;
-            int Q0 = dst_buffer.p[sample_idx].imag();
-            int I1 = dst_buffer.p[sample_idx + 1].real();
-            int Q1 = dst_buffer.p[sample_idx + 1].imag();
-
-            bit_decision = (I0 * Q1 - I1 * Q0) > 0 ? 1 : 0;
-            rb_buf[packet_index] = rb_buf[packet_index] | (bit_decision << j);
-
-            sample_idx += SAMPLE_PER_SYMBOL;
-        }
-
-        packet_index++;
     }
 
     scramble_byte(rb_buf + 2, num_demod_byte, scramble_table[channel_number] + 2, rb_buf + 2);
@@ -310,6 +296,8 @@ void BTLERxProcessor::handlePDUPayloadState() {
             // Skip Header Byte and MAC Address
             uint8_t startIndex = 8;
 
+            int i;
+
             for (i = 0; i < payload_len - 6; i++) {
                 blePacketData.data[i] = rb_buf[startIndex++];
             }
@@ -322,35 +310,40 @@ void BTLERxProcessor::handlePDUPayloadState() {
         }
     }
 
-    parseState = Parse_State_Begin;
+    resetToDefaultState();
 }
 
 void BTLERxProcessor::execute(const buffer_c8_t& buffer) {
     if (!configured) return;
 
-    // Pulled this implementation from channel_stats_collector.c to time slice a specific packet's dB.
+    max_dB = -128;
-    uint32_t max_squared = 0;
 
-    void* src_p = buffer.p;
+    real = -128;
+    imag = -128;
 
-    while (src_p < &buffer.p[buffer.count]) {
+    auto* ptr = buffer.p;
-        const uint32_t sample = *__SIMD32(src_p)++;
+    auto* end = &buffer.p[buffer.count];
-        const uint32_t mag_sq = __SMUAD(sample, sample);
+
-        if (mag_sq > max_squared) {
+    while (ptr < end) {
-            max_squared = mag_sq;
+        float dbm = mag2_to_dbm_8bit_normalized(ptr->real(), ptr->imag(), 1.0f, 50.0f);
-        }
+
+        if (dbm > max_dB) {
+            max_dB = dbm;
+            real = ptr->real();
+            imag = ptr->imag();
         }
 
-    const float max_squared_f = max_squared;
+        ptr++;
-    max_dB = mag2_to_dbv_norm(max_squared_f * (1.0f / (32768.0f * 32768.0f)));
+    }
 
     // 4Mhz 2048 samples
     // Decimated by 4 to achieve 2048/4 = 512 samples at 1 sample per symbol.
     decim_0.execute(buffer, dst_buffer);
     feed_channel_stats(dst_buffer);
 
-    symbols_eaten = 0;
+    samples_eaten = 0;
 
+    while (samples_eaten < (int)dst_buffer.count) {
         // Handle parsing based on parseState
         if (parseState == Parse_State_Begin) {
             handleBeginState();
@@ -363,6 +356,7 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) {
         if (parseState == Parse_State_PDU_Payload) {
             handlePDUPayloadState();
         }
+    }
 }
 
 void BTLERxProcessor::on_message(const Message* const message) {
@@ -372,7 +366,7 @@ void BTLERxProcessor::on_message(const Message* const message) {
 
 void BTLERxProcessor::configure(const BTLERxConfigureMessage& message) {
     channel_number = message.channel_number;
-    decim_0.configure(taps_BTLE_1M_PHY_decim_0.taps);
+    decim_0.configure(taps_BTLE_2M_PHY_decim_0.taps);
 
     configured = true;
 

firmware/baseband/proc_btlerx.hpp

@@ -47,6 +47,8 @@ class BTLERxProcessor : public BasebandProcessor {
     static constexpr int LEN_DEMOD_BUF_ACCESS{32};
     static constexpr uint32_t DEFAULT_ACCESS_ADDR{0x8E89BED6};
     static constexpr int NUM_ACCESS_ADDR_BYTE{4};
+    static constexpr int NUM_PDU_HEADER_BYTE{2};
+    static constexpr int ROLLING_WINDOW{32};
 
     enum Parse_State {
         Parse_State_Begin = 0,
@@ -81,8 +83,8 @@ class BTLERxProcessor : public BasebandProcessor {
     };
 
     static constexpr size_t baseband_fs = 4000000;
-    static constexpr size_t audio_fs = baseband_fs / 8 / 8 / 2;
 
+    float get_phase_diff(const complex16_t& sample0, const complex16_t& sample1);
     uint_fast32_t crc_update(uint_fast32_t crc, const void* data, size_t data_len);
     uint_fast32_t crc24_byte(uint8_t* byte_in, int num_byte, uint32_t init_hex);
     bool crc_check(uint8_t* tmp_byte, int body_len, uint32_t crc_init);
@@ -95,6 +97,12 @@ class BTLERxProcessor : public BasebandProcessor {
     // void demod_byte(int num_byte, uint8_t *out_byte);
     int verify_payload_byte(int num_payload_byte, ADV_PDU_TYPE pdu_type);
 
+    void resetOffsetTracking();
+    void resetBitPacketIndex();
+    void resetToDefaultState();
+
+    void demodulateFSKBits(int num_demod_byte);
+
     void handleBeginState();
     void handlePDUHeaderState();
     void handlePDUPayloadState();
@@ -120,13 +128,20 @@ class BTLERxProcessor : public BasebandProcessor {
     BlePacketData blePacketData{};
 
     Parse_State parseState{Parse_State_Begin};
-    uint16_t packet_index{0};
+    int samples_eaten{0};
-    int sample_idx{0};
-    int symbols_eaten{0};
     uint8_t bit_decision{0};
     uint8_t payload_len{0};
     uint8_t pdu_type{0};
     int32_t max_dB{0};
+    int8_t real{0};
+    int8_t imag{0};
+    uint16_t packet_index{0};
+    uint8_t bit_index{0};
+
+    float frequency_offset_estimate{0.0f};
+    float frequency_offset{0.0f};
+    float phase_buffer[ROLLING_WINDOW] = {0.0f};
+    int phase_buffer_index = 0;
 
     /* NB: Threads should be the last members in the class definition. */
     BasebandThread baseband_thread{baseband_fs, this, baseband::Direction::Receive};

firmware/common/utility.cpp

@@ -91,6 +91,26 @@ float mag2_to_dbv_norm(const float mag2) {
     return (fast_log2(mag2) - mag2_log2_max) * mag2_to_db_factor;
 }
 
+// Function to calculate dBm and normalize based on LNA and VGA settings
+float mag2_to_dbm_8bit_normalized(int8_t real, int8_t imag, float v_ref, float R) {
+    // Step 1: Normalize IQ values (convert 8-bit signed to -1.0 to +1.0)
+    float I = real / 127.0f;  // Map the 8-bit real part to the [-1, 1] range
+    float Q = imag / 127.0f;  // Map the 8-bit imaginary part to the [-1, 1] range
+
+    // Step 2: Compute the magnitude squared (I^2 + Q^2)
+    float mag2 = I * I + Q * Q;
+
+    // Step 3: Convert the magnitude squared to actual power (in watts)
+    float voltage_squared = mag2 * v_ref * v_ref;
+    float power_watts = voltage_squared / R;
+
+    // Step 4: Convert the power to dBm (multiply by 1000 to convert watts to milliwatts)
+    float power_milliwatts = power_watts * 1000.0f;
+    float dbm_measured = 10.0f * log10f(power_milliwatts);
+
+    return dbm_measured;
+}
+
 // Integer in and out approximation
 // >40 times faster float sqrt(x*x+y*y) on Cortex M0
 // derived from https://dspguru.com/dsp/tricks/magnitude-estimator/

firmware/common/utility.hpp

@@ -90,6 +90,7 @@ float fast_log2(const float val);
 float fast_pow2(const float val);
 
 float mag2_to_dbv_norm(const float mag2);
+float mag2_to_dbm_8bit_normalized(int8_t real, int8_t imag, float v_ref, float R);
 
 inline float magnitude_squared(const std::complex<float> c) {
     const auto r = c.real();