00001 import struct 00002 import threading 00003 from BAL.Timers.watch_dog_timer import WatchDogTimer 00004 from BAL.protocol.packages.channel_respond import ChannelRespond 00005 from BAL.protocol.packages.clib_status import ClibStatus, CANCEL, SAVE, RESET 00006 from BAL.protocol.serial_hadler import SerialHandler 00007 from BAL.protocol.packages.header import CONFIG_HEADER, LOAD_HEADER, HEADER_START, DONE_HEADER 00008 from BAL.protocol.packages.channel_to_takeover import ChannelToTakeover, CHANNEL_TO_TAKEOVER 00009 from BAL.protocol.packages.motor_to_takeover import MotorToTakeover, MOTOR_TO_TAKEOVER 00010 00011 00012 class ProtocolHandler(object): 00013 def __init__(self, dev): 00014 self._serial_handler = SerialHandler(dev) 00015 self._forward_clib = False 00016 self._turn_clib = False 00017 00018 self._lock = threading.RLock() 00019 00020 def is_forward_clib(self): 00021 with self._lock: 00022 return self._forward_clib 00023 00024 def set_forward_clib(self, value): 00025 with self._lock: 00026 self._forward_clib = value 00027 00028 def is_turn_clib(self): 00029 with self._lock: 00030 return self._turn_clib 00031 00032 def set_turn_clib(self, value): 00033 with self._lock: 00034 self._turn_clib = value 00035 00036 def in_config_mode(self): 00037 self._serial_handler.write(struct.pack('<B', CONFIG_HEADER)) 00038 00039 def send_clib_status(self, status): 00040 self._serial_handler.write(ClibStatus(status).to_bytes()) 00041 00042 def load_channel_to_takeover(self): 00043 res = ChannelToTakeover() 00044 00045 to_quit = False 00046 while not to_quit: 00047 self.wait_for_header() 00048 res.convert_to_pkg(self._serial_handler.read(res.get_length())) 00049 if res.get_id() == CHANNEL_TO_TAKEOVER: 00050 to_quit = True 00051 00052 return res 00053 00054 def load_motor_to_takeover(self): 00055 res = MotorToTakeover() 00056 00057 to_quit = False 00058 while not to_quit: 00059 self.wait_for_header() 00060 res.convert_to_pkg(self._serial_handler.read(res.get_length())) 00061 if res.get_id() == MOTOR_TO_TAKEOVER: 00062 to_quit = True 00063 return res 00064 00065 def send_channel_to_takeover_pkg(self, ch, listen_mode, value, status): 00066 self._serial_handler.write(ChannelToTakeover(ch, listen_mode, value, status).to_bytes()) 00067 00068 def wait_for_header(self): 00069 while ord(self._serial_handler.read(1)) != HEADER_START: pass 00070 00071 def check_header_or_done(self): 00072 while True: 00073 read = ord(self._serial_handler.read(1)) 00074 if read == HEADER_START: 00075 return True 00076 elif read == DONE_HEADER: 00077 return False 00078 00079 def send_motor_to_takeover_and_clib(self, ch, max_command, pin, tab, min_joy, max_joy, to_clib, deadband, reverse, sp): 00080 self.set_forward_clib(True) 00081 self._serial_handler.write(MotorToTakeover(ch, max_command, pin, tab, min_joy, 0, max_joy, to_clib, deadband, reverse).to_bytes()) 00082 self.wait_for_header() 00083 00084 while self.check_header_or_done(): 00085 res = ChannelRespond() 00086 res.convert_to_pkg(self._serial_handler.read(res.get_length())) 00087 sp.setValue(res.get_value()) 00088 if min_joy > res.get_value(): 00089 min_joy = res.get_value() 00090 sp.setMinimum(min_joy) 00091 elif max_joy < res.get_value(): 00092 max_joy = res.get_value() 00093 sp.setMaximum(max_joy) 00094 00095 self._serial_handler.flush() 00096 00097 def send_motor_to_takeover(self, ch, max_command, pin, tab, min_joy, mid_joy, max_joy, to_clib, deadband, reverse): 00098 00099 self._serial_handler.write(MotorToTakeover(ch, max_command, pin, tab, min_joy, mid_joy, max_joy, to_clib, deadband, reverse).to_bytes()) 00100 00101 def send_read_param(self): 00102 self._serial_handler.write(struct.pack('<B', LOAD_HEADER)) 00103 00104 def got_done(self): 00105 wd = WatchDogTimer(3000) 00106 now = WatchDogTimer.now() 00107 while not self._serial_handler.available() and not wd.check_if_timeout(now): 00108 now = WatchDogTimer.now() 00109 00110 if not wd.is_timeout(): 00111 return ord(self._serial_handler.read(1)) == DONE_HEADER 00112 return False