updating streaming

code/l452_code/packet_parser.py

@@ -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

@@ -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)
+    types = get_type_list(packet_definitions)
-
-    adcs = []
-    accs = []
-    gyros = []
     
     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
+        assert len(cons) <= size
-            while (index < 1024):
+        if len(cons) == size:
-                packet_type = (cons[index + 1]<<8) + cons[index]
+            read_and_process(types, cons, size)                
-                print(packet_type, index)#, len(cons))
+            cons = bytes()
-                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']
 
 async def main():
     device = await scan()

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']
+
+            

code/l452_code/packet_parser_serial.py

@@ -4,85 +4,7 @@ from scipy import signal
 import time
 import serial
 
-def arr_sizes(s):
+from packet_parser_helpers import *
-    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()
-                    
                 
 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
+        read_and_process(types, cons, size)
-        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']
-            
             # 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')