ggw/chest_strap
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

updated parsing

Browse Source
This commit is contained in:
ggw
2026-05-09 14:16:20 -07:00
parent 5937f9ac3b
commit 74debab835
3 changed files with 94 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
code/l452_code/packet_parser_ble.py
+12 -8
View File
@@ -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:
code/l452_code/packet_parser_helpers.py
+81 -8
View File
@@ -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']
code/l452_code/packet_parser_serial.py
+1 -42
View File
@@ -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)