From d5ea0f0369033aeb5065c0dbc122f095f78dfddb Mon Sep 17 00:00:00 2001 From: Netro <146584182+iNetro@users.noreply.github.com> Date: Wed, 25 Jun 2025 13:14:04 -0400 Subject: [PATCH] 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 | 291 ++++++++++++++++------- firmware/application/apps/ble_rx_app.hpp | 21 +- firmware/application/log_file.cpp | 4 +- firmware/application/log_file.hpp | 3 +- firmware/baseband/proc_btlerx.cpp | 248 ++++++++++--------- firmware/baseband/proc_btlerx.hpp | 23 +- firmware/common/utility.cpp | 20 ++ firmware/common/utility.hpp | 1 + 8 files changed, 382 insertions(+), 229 deletions(-) diff --git a/firmware/application/apps/ble_rx_app.cpp b/firmware/application/apps/ble_rx_app.cpp index 4345bd954..2e06f41c7 100644 --- a/firmware/application/apps/ble_rx_app.cpp +++ b/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::draw( if (!entry.nameString.empty() && entry.include_name) { line = entry.nameString; - if (line.length() < 17) { - line += pad_string_with_spaces(17 - line.length()); + if (line.length() < 10) { + 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(); 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,10 +874,49 @@ bool BLERxView::saveFile(const std::filesystem::path& path) { } void BLERxView::on_data(BlePacketData* packet) { - if (!logging) { - str_log = ""; + 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); + + // Preserve exisisting data from entry. + tempEntry.macAddress = macAddressEncoded; + tempEntry.numHits = ++entry.numHits; + + entry = tempEntry; + + handle_filter_options(options_filter.selected_index()); + handle_entries_sort(options_sort.selected_index()); + + if (!searchList.empty()) { + auto it = searchList.begin(); + + while (it != searchList.end()) { + std::string searchStr = (std::string)*it; + + if (entry.dataString.find(searchStr) != std::string::npos) { + searchList.erase(it); + found_count++; + break; + } + + it++; + } + + 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); @@ -792,49 +930,14 @@ void BLERxView::on_data(BlePacketData* packet) { 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. - auto& entry = ::on_packet(recent, macAddressEncoded & 0xFFFFFFFFFFFF); - updateEntry(packet, entry, (ADV_PDU_TYPE)packet->type); - - // Add entries if they meet the criteria. - // auto value = filter; - // resetFilteredEntries(recent, [&value](const BleRecentEntry& entry) { - // return (entry.dataString.find(value) == std::string::npos) && (entry.nameString.find(value) == std::string::npos); - // }); - 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"); + logger->log_raw_data(str_console); } if (serial_logging) { UsbSerialAsyncmsg::asyncmsg(str_console); // new line handled there, no need here. } - str_console = ""; - - if (!searchList.empty()) { - auto it = searchList.begin(); - - while (it != searchList.end()) { - std::string searchStr = (std::string)*it; - - if (entry.dataString.find(searchStr) != std::string::npos) { - searchList.erase(it); - found_count++; - break; - } - - it++; - } - - text_found_count.set(to_string_dec_uint(found_count) + "/" + to_string_dec_uint(total_count)); - } } 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()) { - ADV_PDU_PAYLOAD_TYPE_0_2_4_6* advertiseData = (ADV_PDU_PAYLOAD_TYPE_0_2_4_6*)entry.packetData.data; - - uint8_t currentByte = 0; - uint8_t length = 0; - 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; - } + if (pdu_type == ADV_IND || pdu_type == ADV_NONCONN_IND) // || pdu_type == SCAN_RSP || pdu_type == ADV_SCAN_IND) + { + if (uniqueParsing) { + // Add your unique beacon parsing function here. } + + if (!success && !uniqueParsing) { + success = parse_beacon_data(packet->data, packet->dataLen, entry.nameString, entry.informationString); + } + } 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 */ diff --git a/firmware/application/apps/ble_rx_app.hpp b/firmware/application/apps/ble_rx_app.hpp index 5d6ae25ae..34b8565ec 100644 --- a/firmware/application/apps/ble_rx_app.hpp +++ b/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 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}, - {"Hits", 7}, - {"dB", 4}, + {"Name", 10}, + {"Information", 13}, + {"dBm", 4}, }}; BleRecentEntriesView recent_entries_view{columns, recent}; diff --git a/firmware/application/log_file.cpp b/firmware/application/log_file.cpp index 61c6bbd22..105fd7bad 100644 --- a/firmware/application/log_file.cpp +++ b/firmware/application/log_file.cpp @@ -28,10 +28,10 @@ Optional LogFile::write_entry(const std::string& entry) { Optional 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 LogFile::write_line(const std::string& message) { +Optional LogFile::write_raw(const std::string& message) { auto error = file.write_line(message); if (!error) { file.sync(); diff --git a/firmware/application/log_file.hpp b/firmware/application/log_file.hpp index 430bf14ae..423552e38 100644 --- a/firmware/application/log_file.hpp +++ b/firmware/application/log_file.hpp @@ -39,11 +39,10 @@ class LogFile { Optional write_entry(const std::string& entry); Optional write_entry(const rtc::RTC& datetime, const std::string& entry); + Optional write_raw(const std::string& message); private: File file{}; - - Optional write_line(const std::string& message); }; #endif /*__LOG_FILE_H__*/ diff --git a/firmware/baseband/proc_btlerx.cpp b/firmware/baseband/proc_btlerx.cpp index c8432aa93..24c3f646f 100644 --- a/firmware/baseband/proc_btlerx.cpp +++ b/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. - - 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 validAccessAddress = DEFAULT_ACCESS_ADDR; 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 = 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 i = samples_eaten; i < (int)dst_buffer.count; i += SAMPLE_PER_SYMBOL) { + float phaseDiff = 0; for (int j = 0; j < SAMPLE_PER_SYMBOL; j++) { - // Sample and compare with the adjacent next sample. - 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; - - demod_buf_access[phase_idx][demod_buf_offset] = (I0 * Q1 - I1 * Q0) > 0 ? 1 : 0; - - int k = sp; - unequal_flag = false; - - accesssAddress = 0; - - 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; - } + phaseDiff += get_phase_diff(dst_buffer.p[i + j], dst_buffer.p[i + j + 1]); } - if (unequal_flag == false) { + phase_buffer[phase_buffer_index] = phaseDiff / (SAMPLE_PER_SYMBOL); + phase_buffer_index = (phase_buffer_index + 1) % ROLLING_WINDOW; + + bool bitDecision = (phaseDiff > 0); + + accesssAddress = (accesssAddress >> 1 | (bitDecision << 31)); + + 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; + 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; - - 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; + samples_eaten += hit_idx; parseState = Parse_State_PDU_Header; } void BTLERxProcessor::handlePDUHeaderState() { - int num_demod_byte = 2; // PDU header has 2 octets - - symbols_eaten += 8 * num_demod_byte * SAMPLE_PER_SYMBOL; - - if (symbols_eaten > (int)dst_buffer.count) { + if (samples_eaten > (int)dst_buffer.count) { return; } - // Jump back down to the beginning of PDU header. - sample_idx = symbols_eaten - (8 * num_demod_byte * SAMPLE_PER_SYMBOL); + demodulateFSKBits(NUM_PDU_HEADER_BYTE); - packet_index = 0; - - 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++; + if (packet_index < NUM_PDU_HEADER_BYTE || bit_index != 0) { + return; } - scramble_byte(rb_buf, num_demod_byte, scramble_table[channel_number], rb_buf); + scramble_byte(rb_buf, 2, 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; - int num_demod_byte = (payload_len + 3); - symbols_eaten += 8 * num_demod_byte * SAMPLE_PER_SYMBOL; + const int num_demod_byte = (payload_len + 3); - if (symbols_eaten > (int)dst_buffer.count) { + if (samples_eaten > (int)dst_buffer.count) { return; } - for (i = 0; i < num_demod_byte; i++) { - rb_buf[packet_index] = 0; + demodulateFSKBits(num_demod_byte + NUM_PDU_HEADER_BYTE); - 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++; + if (packet_index < (num_demod_byte + NUM_PDU_HEADER_BYTE) || bit_index != 0) { + return; } 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,46 +310,52 @@ 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. - uint32_t max_squared = 0; + max_dB = -128; - void* src_p = buffer.p; + real = -128; + imag = -128; - while (src_p < &buffer.p[buffer.count]) { - const uint32_t sample = *__SIMD32(src_p)++; - const uint32_t mag_sq = __SMUAD(sample, sample); - if (mag_sq > max_squared) { - max_squared = mag_sq; + auto* ptr = buffer.p; + auto* end = &buffer.p[buffer.count]; + + while (ptr < end) { + 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; - max_dB = mag2_to_dbv_norm(max_squared_f * (1.0f / (32768.0f * 32768.0f))); + ptr++; + } // 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; - // Handle parsing based on parseState - if (parseState == Parse_State_Begin) { - handleBeginState(); - } + while (samples_eaten < (int)dst_buffer.count) { + // Handle parsing based on parseState + if (parseState == Parse_State_Begin) { + handleBeginState(); + } - if (parseState == Parse_State_PDU_Header) { - handlePDUHeaderState(); - } + if (parseState == Parse_State_PDU_Header) { + handlePDUHeaderState(); + } - if (parseState == Parse_State_PDU_Payload) { - handlePDUPayloadState(); + if (parseState == Parse_State_PDU_Payload) { + handlePDUPayloadState(); + } } } @@ -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; diff --git a/firmware/baseband/proc_btlerx.hpp b/firmware/baseband/proc_btlerx.hpp index d1e6fec8c..10e8a878a 100644 --- a/firmware/baseband/proc_btlerx.hpp +++ b/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 sample_idx{0}; - int symbols_eaten{0}; + int samples_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}; diff --git a/firmware/common/utility.cpp b/firmware/common/utility.cpp index 36446e0ad..c4af7f400 100644 --- a/firmware/common/utility.cpp +++ b/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/ diff --git a/firmware/common/utility.hpp b/firmware/common/utility.hpp index 9eab69e6d..363ef0f27 100644 --- a/firmware/common/utility.hpp +++ b/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 c) { const auto r = c.real();