def __init__(self, uartDev, usartBaundRate, logWriter): self.uartController = UartController(uartDev, usartBaundRate) self.uartOpened = True self.logWriter = logWriter self.onReceiveEvent = self.defaultOnReceiveEvent self.streamProcessor = StreamProcessor(self.onReceiveControl, self.onReceiveSignal, self.onReceiveAutopilot) self.receivingThread = TimerThread('receivingThread', self.receiveThread, 0.05) # 20 Hz self.receivingThread.start()
def __init__(self,uartDev, usartBaundRate, logWriter): self.uartController = UartController(uartDev, usartBaundRate) self.uartOpened = True self.logWriter = logWriter self.onReceiveEvent = self.defaultOnReceiveEvent self.streamProcessor = StreamProcessor(self.onReceiveControl, self.onReceiveSignal, self.onReceiveAutopilot) self.receivingThread = TimerThread('receivingThread',self.receiveThread, 0.05) # 20 Hz self.receivingThread.start()
class DroneController: uartController = None receivingThread = None logWriter = None noDebugReceivingCounter = 0 maxTimeoutCounter = 20 # 20 * 0.05 = 1s onReceiveEvent = None # Event to call when read data from USART streamProcessor = None uartOpened = None def __init__(self,uartDev, usartBaundRate, logWriter): self.uartController = UartController(uartDev, usartBaundRate) self.uartOpened = True self.logWriter = logWriter self.onReceiveEvent = self.defaultOnReceiveEvent self.streamProcessor = StreamProcessor(self.onReceiveControl, self.onReceiveSignal, self.onReceiveAutopilot) self.receivingThread = TimerThread('receivingThread',self.receiveThread, 0.05) # 20 Hz self.receivingThread.start() #onReceiveEvent - call this function when new CommData is received via UART def setOnReceiveEvent(self, onReceiveEvent): self.onReceiveEvent = onReceiveEvent def defaultOnReceiveEvent(self, commData): self.logWriter.noteEvent('DroneController: defaultOnReceiveEvent' + str(commData)) def close(self): print('DroneControler: close') if self.uartOpened: print ('DroneControler: UART opened, sending STOP command') self.sendCommData(ControlData.StopCommand()) # wait for sending and response time.sleep(1.5) TimerThread.kill(self.receivingThread) self.uartController.close() self.uartOpened = False # Thread # uartConnection # handler for receiving thread def receiveThread(self): data = self.uartController.recv() dataLength = len(data) if dataLength == 0: self.noDebugReceivingCounter += 1 if self.noDebugReceivingCounter >= self.maxTimeoutCounter: # build ERROR_CONNECTION DebugData and call onReceiveEvent debugData = DebugData.SomeValidDebugMessage() debugData.setConnectionLost() self.logWriter.noteEvent('DroneController: Error! Receiving thread timeout.') self.onReceiveEvent(debugData) self.noDebugReceivingCounter = 0 return self.noDebugReceivingCounter = 0 log_msg = 'DroneController: received: [0x' + str(data.encode("hex")) + ']' self.logWriter.noteEvent(log_msg) # push data to stream processor - when data packet is be received onReceive event will be called self.streamProcessor.processStream(data) # event called by StreamProcessor - on control preamble def onReceiveControl(self, debugDataMsg): debugData = DebugData(debugDataMsg) if debugData.isValid(): # forward data to DronController self.onReceiveEvent(debugData) log_msg = 'DroneController: DebugData received: [' + str(debugData) + ']' else: log_msg = 'DroneController: INVALID DebugData received: [' + debugDataMsg + ']' self.logWriter.noteEvent(log_msg) # event called by StreamProcessor - on signal preamble def onReceiveSignal(self, signalPongMsg): # this event should never be called on Uart communication side log_msg = 'DroneController: Unexpected event received [' + str(signalPongMsg.encode("hex")) + ']' self.logWriter.noteEvent(log_msg) # event called by StreamProcessor - on autopilota preamble def onReceiveAutopilot(self, autopilotDataMsg): autopilotData = AutopilotData(autopilotDataMsg) if autopilotData.isValid(): # forward data to DroneController self.onReceiveEvent(autopilotData) log_msg = 'DroneController: AutopilotData received [0x' + str(autopilotDataMsg.encode("hex")) + ']' else: log_msg = 'DroneController: INVALID AutopilotData received [0x' + autopilotDataMsg + ']' self.logWriter.noteEvent(log_msg) # send CommData to drone def sendCommData(self, commData): log_msg = 'DroneController: Forwarding CommData: [0x' + commData.data.encode("hex") + ']' self.logWriter.noteEvent(log_msg) self.uartController.send(commData.data) def getDebugData(self): return self.debugData
class DroneController: uartController = None receivingThread = None logWriter = None noDebugReceivingCounter = 0 maxTimeoutCounter = 20 # 20 * 0.05 = 1s onReceiveEvent = None # Event to call when read data from USART streamProcessor = None uartOpened = None def __init__(self, uartDev, usartBaundRate, logWriter): self.uartController = UartController(uartDev, usartBaundRate) self.uartOpened = True self.logWriter = logWriter self.onReceiveEvent = self.defaultOnReceiveEvent self.streamProcessor = StreamProcessor(self.onReceiveControl, self.onReceiveSignal, self.onReceiveAutopilot) self.receivingThread = TimerThread('receivingThread', self.receiveThread, 0.05) # 20 Hz self.receivingThread.start() #onReceiveEvent - call this function when new CommData is received via UART def setOnReceiveEvent(self, onReceiveEvent): self.onReceiveEvent = onReceiveEvent def defaultOnReceiveEvent(self, commData): self.logWriter.noteEvent('DroneController: defaultOnReceiveEvent' + str(commData)) def close(self): print('DroneControler: close') if self.uartOpened: print('DroneControler: UART opened, sending STOP command') self.sendCommData(ControlData.StopCommand()) # wait for sending and response time.sleep(1.5) TimerThread.kill(self.receivingThread) self.uartController.close() self.uartOpened = False # Thread # uartConnection # handler for receiving thread def receiveThread(self): data = self.uartController.recv() dataLength = len(data) if dataLength == 0: self.noDebugReceivingCounter += 1 if self.noDebugReceivingCounter >= self.maxTimeoutCounter: # build ERROR_CONNECTION DebugData and call onReceiveEvent debugData = DebugData.SomeValidDebugMessage() debugData.setConnectionLost() self.logWriter.noteEvent( 'DroneController: Error! Receiving thread timeout.') self.onReceiveEvent(debugData) self.noDebugReceivingCounter = 0 return self.noDebugReceivingCounter = 0 log_msg = 'DroneController: received: [0x' + str( data.encode("hex")) + ']' self.logWriter.noteEvent(log_msg) # push data to stream processor - when data packet is be received onReceive event will be called self.streamProcessor.processStream(data) # event called by StreamProcessor - on control preamble def onReceiveControl(self, debugDataMsg): debugData = DebugData(debugDataMsg) if debugData.isValid(): # forward data to DronController self.onReceiveEvent(debugData) log_msg = 'DroneController: DebugData received: [' + str( debugData) + ']' else: log_msg = 'DroneController: INVALID DebugData received: [' + debugDataMsg + ']' self.logWriter.noteEvent(log_msg) # event called by StreamProcessor - on signal preamble def onReceiveSignal(self, signalPongMsg): # this event should never be called on Uart communication side log_msg = 'DroneController: Unexpected event received [' + str( signalPongMsg.encode("hex")) + ']' self.logWriter.noteEvent(log_msg) # event called by StreamProcessor - on autopilota preamble def onReceiveAutopilot(self, autopilotDataMsg): autopilotData = AutopilotData(autopilotDataMsg) if autopilotData.isValid(): # forward data to DroneController self.onReceiveEvent(autopilotData) log_msg = 'DroneController: AutopilotData received [0x' + str( autopilotDataMsg.encode("hex")) + ']' else: log_msg = 'DroneController: INVALID AutopilotData received [0x' + autopilotDataMsg + ']' self.logWriter.noteEvent(log_msg) # send CommData to drone def sendCommData(self, commData): log_msg = 'DroneController: Forwarding CommData: [0x' + commData.data.encode( "hex") + ']' self.logWriter.noteEvent(log_msg) self.uartController.send(commData.data) def getDebugData(self): return self.debugData