From 74debab835b3c5153adcb3a183e15ce0de785515 Mon Sep 17 00:00:00 2001 From: ggw Date: Sat, 9 May 2026 14:16:20 -0700 Subject: [PATCH] updated parsing --- code/l452_code/packet_parser_ble.py | 20 +++--- code/l452_code/packet_parser_helpers.py | 89 ++++++++++++++++++++++--- code/l452_code/packet_parser_serial.py | 43 +----------- 3 files changed, 94 insertions(+), 58 deletions(-) diff --git a/code/l452_code/packet_parser_ble.py b/code/l452_code/packet_parser_ble.py index 9a6cc1d..03da5cc 100644 --- a/code/l452_code/packet_parser_ble.py +++ b/code/l452_code/packet_parser_ble.py @@ -12,8 +12,9 @@ async def scan(): async def connect(device): async with BleakClient(device) as client: - value = bytes([0x01]) - #await client.write_gatt_char(TX_UUID, value, response=True) + value = b'6' + await client.write_gatt_char(TX_UUID, value, response=True) + await client.stop_notify(RX_UUID) await client.start_notify(RX_UUID, cb) await asyncio.sleep(10000) @@ -28,19 +29,22 @@ def cb(sender, data): async def update_with_data(): types = get_type_list(packet_definitions) - + cons = bytes() + start = time.time() while(True): data = await queue.get() if len(cons) == 0: size = int.from_bytes(data[:4], byteorder = 'little', signed = False) cons = cons + data - - assert len(cons) <= size - if len(cons) == size: - read_and_process(types, cons, size) + if len(cons) == size + 4: + read_and_process(types, cons[4 : size + 4], size) cons = bytes() - + start = time.time() + if len(cons) > size + 4: + print("Misaligned") + cons = bytes() + async def main(): device = await scan() if not device: diff --git a/code/l452_code/packet_parser_helpers.py b/code/l452_code/packet_parser_helpers.py index cde5446..befe946 100644 --- a/code/l452_code/packet_parser_helpers.py +++ b/code/l452_code/packet_parser_helpers.py @@ -1,10 +1,12 @@ +import numpy as np +import matplotlib.pyplot as plt -packet_definitions = """packet_rtc 1 28 uint32_t t 0 4 RTC_TimeTypeDef sTime 4 20 RTC_DateTypeDef sDate 24 4 +packet_definitions = b"""packet_rtc 1 28 uint32_t t 0 4 RTC_TimeTypeDef sTime 4 20 RTC_DateTypeDef sDate 24 4 packet_vbatt 2 8 uint32_t t 0 4 uint16_t vbatt_cnts 4 2 packet_imu 3 148 uint32_t t 0 4 uint8_t data[141] 4 1 packet_adc 4 268 uint32_t t 0 4 uint8_t index 4 1 int32_t ekg_readings_cnts[50] 8 4 int32_t str_readings_cnts[5] 208 4 int32_t oT_readings_cnts[5] 228 4 int32_t iT_readings_cnts[5] 248 4 packet_spo2 5 184 uint32_t t 0 4 uint8_t bytes[180] 4 1 -packet_msg 6 36 uint32_t t 0 4 char buff[32] 4 1""" +packet_msg 6 36 uint32_t t 0 4 char buff[32] 4 1""".split(b'\n') def arr_sizes(s): if s[-1:] != b']' or b'[' not in s: @@ -42,11 +44,36 @@ def my_filter(arr): arr[ind] = 0.5 * arr[prev_ind] + 0.5 * arr[next_ind] return arr +reds = [] +irs = [] +greens = [] +def process_ppg(d, t): + global greens, reds, irs + for e in t['elements']: + block = d[e['offset']:e['offset'] + e['size']] + element_size = int(len(block) / e['n_elements']) + if e['name'] == b'bytes[180]': + reds += [int.from_bytes(block[3 * i : 3 * i + 3], byteorder = 'big') for i in range(0,60,3)] + irs += [int.from_bytes(block[3 * i : 3 * i + 3], byteorder = 'big') for i in range(1,60,3)] + greens += [int.from_bytes(block[3 * i : 3 * i + 3], byteorder = 'big') for i in range(2,60,3)] + if len(reds) > 400: + reds = reds[-400:] + irs = irs[-400:] + greens = greens[-400:] + fig, axs = plt.subplots(3) + axs[0].set_title('red') + axs[1].set_title('ir') + axs[2].set_title('green') + axs[0].plot(reds) + axs[1].plot(irs) + axs[2].plot(greens) + plt.savefig("ppg.png") + plt.close() + accs = [] gyros = [] def process_imu(d, t): - global accs - global gyros + global accs, gyros for e in t['elements']: block = d[e['offset']:e['offset'] + e['size']] element_size = int(len(block) / e['n_elements']) @@ -62,7 +89,8 @@ def process_imu(d, t): elif imu_reading_type == 2: accs.append(4 / (1<<16) * data) else: - assert False + pass + #assert False if len(gyros) > 400: gyros = gyros[-400:] if len(accs) > 400: @@ -81,18 +109,63 @@ def process_imu(d, t): plt.savefig("acc_gyro.png") plt.close() +ecgs = [] +t1s = [] +t2s = [] +strains = [] + +def process_adc(d, t): + global ecgs, t1s, t2s, strains + for e in t['elements']: + block = d[e['offset']:e['offset'] + e['size']] + element_size = int(len(block) / e['n_elements']) + if e['name'] == b'ekg_readings_cnts[50]': + ecgs = ecgs + [(2.4 / (1<<24)) * int.from_bytes(block[4 * i : 4 * i + 4], byteorder = 'little', signed = True) for i in range(50)] + if len(ecgs) > 200: + ecgs = ecgs[-2048:] + t1s = t1s[-205:] + t2s = t2s[-205:] + strains = strains[-205:] + fig, axs = plt.subplots(4) + axs[0].set_title("ECG") + axs[1].set_title("Strain") + axs[2].set_title("oT") + axs[3].set_title("iT") + axs[0].plot(ecgs) + axs[1].plot(strains) + axs[2].plot(t1s) + axs[3].plot(t2s) + plt.savefig("adcs.png") + plt.close() + if e['name'] == b'str_readings_cnts[5]': + strains = strains + [(2.4 / (1<<24)) * int.from_bytes(block[4 * i : 4 * i + 4], byteorder = 'little', signed = True) for i in range(5)] + if e['name'] == b'oT_readings_cnts[5]': + t1s = t1s + [(2.4 / (1<<24)) * int.from_bytes(block[4 * i : 4 * i + 4], byteorder = 'little', signed = True) for i in range(5)] + if e['name'] == b'iT_readings_cnts[5]': + t2s = t2s + [(2.4 / (1<<24)) * int.from_bytes(block[4 * i : 4 * i + 4], byteorder = 'little', signed = True) for i in range(5)] + def read_and_process(types, cons, size): index = 0 while (index < size): packet_type = cons[index] - t = [t for t in types if t['type_code'] == packet_type][0] + print(packet_type) + try: + t = [t for t in types if t['type_code'] == packet_type][0] + except: + print("HERE") + print(cons[index-5:index+5]) + quit() + return print(index, packet_type, t['type_name']) d = cons[index + 1 : index + 1 + t['size']] if t['type_name'] == b'packet_imu': - pass - #process_imu(d, t) + process_imu(d, t) if t['type_name'] == b'packet_msg': print(d) + if t['type_name'] == b'packet_adc': + process_adc(d, t) + if t['type_name'] == b'packet_spo2': + process_ppg(d, t) index += 1 + t['size'] diff --git a/code/l452_code/packet_parser_serial.py b/code/l452_code/packet_parser_serial.py index c1ff6c2..1852ff6 100644 --- a/code/l452_code/packet_parser_serial.py +++ b/code/l452_code/packet_parser_serial.py @@ -16,48 +16,7 @@ def update_with_data(ser, types): size = int.from_bytes(ser.read(4), byteorder = 'little', signed = False) cons = ser.read(size) read_and_process(types, cons, size) - # if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]': - # chunked = [(2.4 / (1<<24)) * int.from_bytes(block[i:i + element_size], byteorder='little', signed = True) for i in range(0, len(block), element_size)] - # adcs += chunked - # if len(adcs) > 500: - # dat = np.array(adcs[-2048:]) - # if len(adcs) > 2048: - # adcs = adcs[-2048:] - # #filtered_dat = my_filter(dat) - # filtered_dat = dat - # filtered_dat = signal.filtfilt(b, a, filtered_dat) - # dd = np.sort(dat) - # center = 0.5 * (dd[-100] + dd[100]) - # range_ = dd[-100] - dd[100] - # if len(dat) == 2048: - # print("Freq:", 2048 / (time.time() - start)) - # axs[0].cla() - # axs[1].cla() - # axs[0].set_title("ADC {:3.3f}mV".format(1000 * (range_))) - - # #axs[0].plot(dat, 'r.', linestyle = '--') - # axs[0].plot(filtered_dat, 'k.', linestyle = '--') - # #axs[0].set_ylim(center - range_, center + range_) - # _fft = np.log(np.abs(np.fft.fft(filtered_dat)[1:])) - # axs[1].plot(250 * np.fft.fftfreq(dat.shape[-1])[1:], _fft, 'k.') - # axs[1].axvline(x = 60) - # plt.savefig("image.png") - - # if t['type_name'] == b'packet_imu' and e['name'] == b'readings_cnts[4]': - # imu_reading_type = block[0] >> 3 - # imu_reading_tag_cnt = (block[0] >> 1) & 3 - # reading_xyz = [int.from_bytes(block[2:4], byteorder = 'big', signed = True), - # int.from_bytes(block[4:6], byteorder = 'big', signed = True), - # int.from_bytes(block[6:8], byteorder = 'big', signed = True)] - # if imu_reading_type == 1: - # #print("{:02x} {:d}".format(imu_reading_type, imu_reading_tag_cnt)) - # gyros.append([250 * e / (1<<16) for e in reading_xyz]) - # elif imu_reading_type == 2: - # accs.append([4 * e / (1<<16) for e in reading_xyz]) - # else: - # print("ERR") - - + def main(): ser = serial.Serial(port = '/dev/ttyACM1', timeout = 0.5)