working on l452

code/l452_code/packet_parser.py

@@ -1,27 +1,81 @@
 import serial
+import matplotlib.pyplot as plt
+import numpy as np
+from scipy import signal
 
-ser = serial.Serial(port='/dev/ttyACM1', baudrate=115200)
+#should be running at 244Hz
+
+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
 
-f = open('data_dump.dat','wb')
+ser.write(b'?')
 
-for i in range(5):
-		ser.write(b'R')
-		data = ser.read(1024)
-		index = 0
-		while (index < 1024):
-				packet_length = (data[index + 1]<<8) + data[index]
-				packet_type = (data[index + 3]<<8) + data[index + 2]
-				print(packet_type, packet_length)
-				if packet_type not in range(1,10):
-						break
-				#print(packet_type, packet_length, data[index + 4: index + 4 + packet_length])
-				index += packet_length + 4
-		ser.flush()
-		# if packet_type in [1,2,3,4,5,6,7,8,9] and packet_length > 6:
-		# 		dat = ser.read(packet_length)
-		# 		print(packet_type, packet_length, dat)
+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
+
+ys = []
+
+for i in range(1000):
+    ser.write(b'R')
+    data = ser.read(1024)
+    index = 0
+    while (index < 1024):
+        packet_type = (data[index + 1]<<8) + data[index]
+        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 = [int.from_bytes(block[i:i + element_size - 1], byteorder='little', signed = True) for i in range(0, len(block), element_size)]
+                #BB = [block[i:i + element_size] for i in range(0, len(block), element_size)]
+                #if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]':
+                ys += chunked
+                plt.clf()
+                dat = np.array(ys[-1000:])
+                plt.plot(dat, 'k.', linestyle = '--')#signal.filtfilt(b, a, dat))
+                plt.pause(0.01)
+                ##print(BB)
+                e['readings'].append(chunked)
+        index += 2 + t['size']
+
+#for t in types:
+#    if t['type_name'] == b'packet_ekg':
+#        print(t)
+