diff --git a/code/l452_code/packet_parser.py b/code/l452_code/packet_parser.py deleted file mode 100644 index dc0aef7..0000000 --- a/code/l452_code/packet_parser.py +++ /dev/null @@ -1,111 +0,0 @@ -import serial -import matplotlib.pyplot as plt -import numpy as np -from scipy import signal -import time - -fs = 244 -b, a = signal.butter( - N = 2, - Wn = [30], - btype = 'lowpass', - fs = fs) - -ser = serial.Serial(port='/dev/ttyACM1', baudrate=115200, timeout = 0.5) -ser.flush() - -data = bytes() - -ready = False - -ser.write(b'?') - -types = [] - -def arr_sizes(s): - if s[-1:] != b']' or b'[' not in s: - return 1 - v = int(s[s.rindex(b'[') + 1:-1]) - return v - -for i in range(13): - line = ser.readline().strip(b'\n').strip(b'\r') - if line != b'': - L = line.split(b" ") - types.append({'type_name' : L[0], - 'type_code' : int(L[1]), - 'size' : int(L[2]), - 'elements' : [] - }) - i = 3 - while i < len(L): - types[-1]['elements'].append({'type_name' : L[i], 'name' : L[i + 1], 'offset' : int(L[i + 2]), 'n_elements' : arr_sizes(L[i + 1]), 'size' : int(L[i + 3]) * arr_sizes(L[i + 1]), 'readings' : []}) - i += 4 - -ser.flush() -ser.timeout = 5 - -adcs = [] -accs = [] -gyros = [] - -fig, axs = plt.subplots(2,2) -then = time.time() -for i in range(10): - ser.write(b'R') - data = ser.read(1024) - continue - - index = 0 - while (index < 1024): - packet_type = (data[index + 1]<<8) + data[index] - #print(packet_type) - ind = [i for i,t in enumerate(types) if t['type_code'] == packet_type] - if len(ind) != 1: - break - t = types[ind[0]] - d = data[index + 2 : index + 2 + t['size']] - for e in t['elements']: - block = d[e['offset']:e['offset'] + e['size']] - element_size = int(len(block) / e['n_elements']) - 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 - axs[0][0].cla() - axs[0][1].cla() - axs[1][1].cla() - dat = np.array(adcs[-1000:]) - axs[0][0].set_title("ADC") - axs[0][1].set_title("ACC") - axs[1][1].set_title("GYRO") - axs[0][0].plot(dat, 'k.', linestyle = '--')#signal.filtfilt(b, a, dat)) - axs[0][1].plot(accs[-30:], 'k.', linestyle = '--')#signal.filtfilt(b, a, dat)) - axs[1][1].plot(gyros[-30:], 'k.', linestyle = '--')#signal.filtfilt(b, a, dat)) - - plt.pause(0.01) - ##print(BB) - e['readings'].append(chunked) - 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 # tag count is sensor time slow - 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") - #chunked = [int.from_bytes(block[i:i + element_size], byteorder='little', signed = True) for i in range(0, len(block), element_size)] - #print(chunked) - #print(t['type_name']) - - index += 2 + t['size'] - -#for t in types: -# if t['type_name'] == b'packet_ekg': -# print(t) - -print((time.time() - then) / 10) diff --git a/code/l452_code/packet_parser_ble.py b/code/l452_code/packet_parser_ble.py index 9f5d771..9a6cc1d 100644 --- a/code/l452_code/packet_parser_ble.py +++ b/code/l452_code/packet_parser_ble.py @@ -2,43 +2,11 @@ import matplotlib.pyplot as plt import numpy as np from scipy import signal import time - -format_string=( - 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 9 12 uint32_t t 0 4 uint16_t readings_cnts[4] 4 2 -packet_ekg 3 208 uint32_t t 0 4 uint8_t index 4 1 int32_t readings_cnts[50] 8 4 -packet_strain 4 28 uint32_t t 0 4 uint8_t index 4 1 int32_t readings_cnts[5] 8 4 -packet_outsideT 5 28 uint32_t t 0 4 uint8_t index 4 1 int32_t readings_cnts[5] 8 4 -packet_insideT 6 28 uint32_t t 0 4 uint8_t index 4 1 int32_t readings_cnts[5] 8 4 -packet_button 7 8 uint32_t t 0 4 uint8_t button_vec 4 1 -packet_spo2 8 304 uint32_t t 0 4 uint32_t green_cnts[25] 4 4 uint32_t red_cnts[25] 104 4 uint32_t ir_cnts[25] 204 4""") - -types = [] - -def arr_sizes(s): - if s[-1:] != b']' or b'[' not in s: - return 1 - v = int(s[s.rindex(b'[') + 1:-1]) - return v - -for line in format_string.split(b"\n"): - line = line.strip(b'\n').strip(b'\r') - if line != b'': - L = line.split(b" ") - types.append({'type_name' : L[0], - 'type_code' : int(L[1]), - 'size' : int(L[2]), - 'elements' : [] - }) - i = 3 - while i < len(L): - types[-1]['elements'].append({'type_name' : L[i], 'name' : L[i + 1], 'offset' : int(L[i + 2]), 'n_elements' : arr_sizes(L[i + 1]), 'size' : int(L[i + 3]) * arr_sizes(L[i + 1]), 'readings' : []}) - i += 4 - import asyncio from bleak import BleakScanner, BleakClient +from packet_parser_helpers import * + async def scan(): return await BleakScanner.find_device_by_name("XX-STM32") @@ -57,89 +25,21 @@ queue = asyncio.Queue() def cb(sender, data): queue.put_nowait(data) -def my_filter(arr): - inds = np.where(np.abs(np.diff(arr)) > 0.005)[0] - for ind in inds: - prev_ind = ind - 1 - while prev_ind in inds: - prev_ind -= 1 - next_ind = ind + 1 - while next_ind in inds: - next_ind += 1 - arr[ind] = 0.5 * arr[prev_ind] + 0.5 * arr[next_ind] - return arr - -# I think this is missing a lot of data (70Hz vs should have 250Hz) - async def update_with_data(): - fig, axs = plt.subplots(2) - - adcs = [] - accs = [] - gyros = [] + 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 - if len(cons) >= 1024: - assert(len(cons) == 1024) - index = 0 - while (index < 1024): - packet_type = (cons[index + 1]<<8) + cons[index] - print(packet_type, index)#, len(cons)) - ind = [i for i,t in enumerate(types) if t['type_code'] == packet_type] - if len(ind) != 1: - if (index == 0): - cons = cons[128:] - else: - cons = bytes() - break - t = types[ind[0]] - d = cons[index + 2 : index + 2 + t['size']] - for e in t['elements']: - block = d[e['offset']:e['offset'] + e['size']] - element_size = int(len(block) / e['n_elements']) - 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:]) - filtered_dat = my_filter(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") - index += 2 + t['size'] + assert len(cons) <= size + if len(cons) == size: + read_and_process(types, cons, size) + cons = bytes() async def main(): device = await scan() diff --git a/code/l452_code/packet_parser_helpers.py b/code/l452_code/packet_parser_helpers.py new file mode 100644 index 0000000..cde5446 --- /dev/null +++ b/code/l452_code/packet_parser_helpers.py @@ -0,0 +1,98 @@ + +packet_definitions = """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""" + +def arr_sizes(s): + if s[-1:] != b']' or b'[' not in s: + return 1 + v = int(s[s.rindex(b'[') + 1:-1]) + return v + +def get_type_list(lines): + + types = [] + + for line in lines: + if line != b'': + L = line.split(b" ") + types.append({'type_name' : L[0], + 'type_code' : int(L[1]), + 'size' : int(L[2]), + 'elements' : [] + }) + i = 3 + while i < len(L): + types[-1]['elements'].append({'type_name' : L[i], 'name' : L[i + 1], 'offset' : int(L[i + 2]), 'n_elements' : arr_sizes(L[i + 1]), 'size' : int(L[i + 3]) * arr_sizes(L[i + 1])}) + i += 4 + return types + +def my_filter(arr): + inds = np.where(np.abs(np.diff(arr)) > 0.005)[0] + for ind in inds: + prev_ind = ind - 1 + while prev_ind in inds: + prev_ind -= 1 + next_ind = ind + 1 + while next_ind in inds: + next_ind += 1 + arr[ind] = 0.5 * arr[prev_ind] + 0.5 * arr[next_ind] + return arr + +accs = [] +gyros = [] +def process_imu(d, t): + global accs + global gyros + 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'data[141]': + for i in range(20): + reading = block[1 + 7 * i : 8 + 7 * i] + #print(reading) + imu_reading_type = reading[0] >> 3 + imu_reading_tag_cnt = (reading[0] >> 1) & 3 + data = np.array([int.from_bytes(reading[2 * i + 1 : 2 * i + 3], byteorder = 'little', signed = True) for i in range(3)]) + if imu_reading_type == 1: + gyros.append(250 / (1<<16) * data) + elif imu_reading_type == 2: + accs.append(4 / (1<<16) * data) + else: + assert False + if len(gyros) > 400: + gyros = gyros[-400:] + if len(accs) > 400: + accs = accs[-400:] + fig, axs = plt.subplots(2) + g = np.array(gyros) + a = np.array(accs) + axs[0].set_ylabel("dps") + axs[0].plot(g[:,0]) + axs[0].plot(g[:,1]) + axs[0].plot(g[:,2]) + axs[1].set_ylabel("g") + axs[1].plot(a[:,0]) + axs[1].plot(a[:,1]) + axs[1].plot(a[:,2]) + plt.savefig("acc_gyro.png") + plt.close() + +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(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) + if t['type_name'] == b'packet_msg': + print(d) + index += 1 + t['size'] + + diff --git a/code/l452_code/packet_parser_serial.py b/code/l452_code/packet_parser_serial.py index 5a8e13d..c1ff6c2 100644 --- a/code/l452_code/packet_parser_serial.py +++ b/code/l452_code/packet_parser_serial.py @@ -4,85 +4,7 @@ from scipy import signal import time import serial -def arr_sizes(s): - if s[-1:] != b']' or b'[' not in s: - return 1 - v = int(s[s.rindex(b'[') + 1:-1]) - return v - -def get_type_list(ser): - ser.write(b'?') - - types = [] - - for i in range(13): - line = ser.readline().strip(b'\n').strip(b'\r') - if line != b'': - L = line.split(b" ") - types.append({'type_name' : L[0], - 'type_code' : int(L[1]), - 'size' : int(L[2]), - 'elements' : [] - }) - i = 3 - while i < len(L): - types[-1]['elements'].append({'type_name' : L[i], 'name' : L[i + 1], 'offset' : int(L[i + 2]), 'n_elements' : arr_sizes(L[i + 1]), 'size' : int(L[i + 3]) * arr_sizes(L[i + 1])}) - i += 4 - return types - -def my_filter(arr): - inds = np.where(np.abs(np.diff(arr)) > 0.005)[0] - for ind in inds: - prev_ind = ind - 1 - while prev_ind in inds: - prev_ind -= 1 - next_ind = ind + 1 - while next_ind in inds: - next_ind += 1 - arr[ind] = 0.5 * arr[prev_ind] + 0.5 * arr[next_ind] - return arr - - - -accs = [] -gyros = [] -def process_imu(d, t): - global accs - global gyros - 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'data[141]': - for i in range(20): - reading = block[1 + 7 * i : 8 + 7 * i] - #print(reading) - imu_reading_type = reading[0] >> 3 - imu_reading_tag_cnt = (reading[0] >> 1) & 3 - data = np.array([int.from_bytes(reading[2 * i + 1 : 2 * i + 3], byteorder = 'little', signed = True) for i in range(3)]) - if imu_reading_type == 1: - gyros.append(250 / (1<<16) * data) - elif imu_reading_type == 2: - accs.append(4 / (1<<16) * data) - else: - assert False - if len(gyros) > 400: - gyros = gyros[-400:] - if len(accs) > 400: - accs = accs[-400:] - fig, axs = plt.subplots(2) - g = np.array(gyros) - a = np.array(accs) - axs[0].set_ylabel("dps") - axs[0].plot(g[:,0]) - axs[0].plot(g[:,1]) - axs[0].plot(g[:,2]) - axs[1].set_ylabel("g") - axs[1].plot(a[:,0]) - axs[1].plot(a[:,1]) - axs[1].plot(a[:,2]) - plt.savefig("acc_gyro.png") - plt.close() - +from packet_parser_helpers import * def update_with_data(ser, types): @@ -93,19 +15,7 @@ def update_with_data(ser, types): ser.write(b'r') size = int.from_bytes(ser.read(4), byteorder = 'little', signed = False) cons = ser.read(size) - index = 0 - while (index < size): - packet_type = cons[index] - t = [t for t in types if t['type_code'] == packet_type][0] - 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) - if t['type_name'] == b'packet_msg': - print(d) - index += 1 + t['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 @@ -153,8 +63,13 @@ def main(): ser = serial.Serial(port = '/dev/ttyACM1', timeout = 0.5) ser.flush() ser.reset_input_buffer() + + ser.write(b'?') + lines = [] + for i in range(13): + lines.append(ser.readline().strip(b'\n').strip(b'\r')) - types = get_type_list(ser) + types = get_type_list(lines) # ser.write(b'1') # ser.write(b'3')