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

updating streaming

Browse Source
This commit is contained in:
ggw
2026-05-08 17:08:07 -05:00
parent 2b5ade4bec
commit 5937f9ac3b
4 changed files with 115 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files
code/l452_code/packet_parser.py
-111
View File
@@ -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)
code/l452_code/packet_parser_ble.py
+9 -109
View File
@@ -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()
code/l452_code/packet_parser_helpers.py
+98
View File
@@ -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']
code/l452_code/packet_parser_serial.py
+8 -93
View File
@@ -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')