mirror of
https://github.com/eried/portapack-mayhem.git
synced 2025-08-08 22:52:27 -04:00
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
This commit is contained in:
Netro
GitHub
parent
22cc311447
commit
d5ea0f0369
8 changed files with 382 additions and 229 deletions
|
|
@ -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) {
|
||||
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<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.
|
||||
// auto value = filter;
|
||||
// resetFilteredEntries(recent, [&value](const BleRecentEntry& entry) {
|
||||
// return (entry.dataString.find(value) == std::string::npos) && (entry.nameString.find(value) == std::string::npos);
|
||||
// });
|
||||
// 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());
|
||||
|
||||
// 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();
|
||||
|
||||
|
|
@ -837,6 +912,34 @@ void BLERxView::on_data(BlePacketData* packet) {
|
|||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
// New filter? Reset list from recent entries.
|
||||
if (filter != 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;
|
||||
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.
|
||||
}
|
||||
|
||||
uint8_t currentByte = 0;
|
||||
uint8_t length = 0;
|
||||
uint8_t type = 0;
|
||||
if (!success && !uniqueParsing) {
|
||||
success = parse_beacon_data(packet->data, packet->dataLen, entry.nameString, entry.informationString);
|
||||
}
|
||||
|
||||
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 */
|
||||
|
|
|
|||
|
|
@ -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},
|
||||
{"Hits", 7},
|
||||
{"dB", 4},
|
||||
{"Name", 10},
|
||||
{"Information", 13},
|
||||
{"dBm", 4},
|
||||
}};
|
||||
|
||||
BleRecentEntriesView recent_entries_view{columns, recent};
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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__*/
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
phaseDiff += get_phase_diff(dst_buffer.p[i + j], dst_buffer.p[i + j + 1]);
|
||||
}
|
||||
|
||||
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;
|
||||
unequal_flag = false;
|
||||
accesssAddress = (accesssAddress >> 1 | (bitDecision << 31));
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
if (packet_index < NUM_PDU_HEADER_BYTE || bit_index != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
packet_index++;
|
||||
}
|
||||
|
||||
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,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.
|
||||
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;
|
||||
|
||||
while (samples_eaten < (int)dst_buffer.count) {
|
||||
// Handle parsing based on parseState
|
||||
if (parseState == Parse_State_Begin) {
|
||||
handleBeginState();
|
||||
|
|
@ -364,6 +357,7 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) {
|
|||
handlePDUPayloadState();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void BTLERxProcessor::on_message(const Message* const message) {
|
||||
if (message->id == Message::ID::BTLERxConfigure)
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -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/
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue