33import zmq
44import zmq .asyncio
55import serial
6-
6+ import struct
7+ from collections import OrderedDict
78from .logger import logger
89
910
@@ -64,6 +65,7 @@ def __init__(
6465 port = "/dev/ttyUSB0" ,
6566 baudrate = 9600 ,
6667 decoder = None ,
68+ validator = None ,
6769 ):
6870 super ().__init__ (monitor_manager )
6971 if serial_instance :
@@ -76,6 +78,7 @@ def __init__(
7678 self .port = self .serial_connection .port
7779 self .baudrate = self .serial_connection .baudrate
7880 self .decoder = decoder
81+ self .validator = validator
7982
8083 def detect_devices (
8184 self ,
@@ -115,9 +118,12 @@ async def start(self, frequency: float = 30):
115118 if self .serial_connection .in_waiting :
116119 # Read available data on the serial port
117120 if data := self .serial_connection .readline ():
118- if self .decoder :
121+ if self .validator :
122+ data = self .validator .validate (data )
123+ if self .decoder and data :
119124 data = self .decoder .decode (data )
120- self .process_update (data )
125+ if data :
126+ self .process_update (data )
121127 await asyncio .sleep (1 / frequency )
122128
123129 except Exception as e :
@@ -151,4 +157,34 @@ def __init__(self, keys, separator=","):
151157
152158 def decode (self , packet ):
153159 data = packet .decode ("utf-8" ).strip ().split (self .separator )
154- return dict (zip (self .keys , data ))
160+ return OrderedDict (zip (self .keys , data ))
161+
162+
163+ class StructDecoder (Decoder ):
164+ def __init__ (self , data_keys , packet_format = None ):
165+ self .packet_format = packet_format
166+ self .data_keys = data_keys
167+
168+ def decode (self , packet ):
169+ if self .packet_format :
170+ packet = struct .unpack (self .packet_format , packet )
171+ if self .data_keys :
172+ packet = OrderedDict (zip (self .data_keys , packet ))
173+ return packet
174+
175+
176+ class Validator :
177+ @abstractmethod
178+ def validate (self , packet ): ...
179+
180+ class WindowValidator (Validator ):
181+ def __init__ (self , window_size = 10 , start_byte = 0xA5 , end_byte = 0x5A ):
182+ self .window_size = window_size
183+ self .start_byte = start_byte
184+ self .end_byte = end_byte
185+
186+ def validate (self , packet ):
187+ if len (packet ) == self .window_size and packet [0 ] == self .start_byte and packet [- 1 ] == self .end_byte :
188+ # Return packet without start and end bytes
189+ return packet [1 :- 1 ]
190+ return False
0 commit comments