class _FMUManager(QObject): """ This class manages the communication with the controller, e.g. the connection, status updates, etc. """ # Update interval in ms UPDATE_INTERVAL = 1000 # This signals is emitted when the board status was updated board_status_updated = pyqtSignal() # This signal is emitted when the board settings are updated board_settings_updated = pyqtSignal() # This signal is emitted when a new data object message was received data_object_received = pyqtSignal() def __init__(self): QObject.__init__(self) # Serial connection self.serial = SerialConnection() self.serial.set_port(0) self.serial.message_received.connect(self._onMessageReceived) # Board status object self.boardStatus = BoardStatus() # Board settings object self.fmuSettings = FmuSettings() # Status reader self.statusReaderThread = None self.statusReaderIsAlive = False self.statusReaderUpdateInterval = 0.25 # Update interval in seconds # Indicates if new data from the board was received #self.hasNewData = False self._logger = Logger() def connect(self, port=0): """Connects to the flight controller""" self.serial.connect() # Start reading the board status self._startStatusReader() # Read once the board settings self.updateBoardSettings() def disconnect(self): """Disconnects from the flight controller""" # Stop the status reader self._stopStatusReader() # Disconnects self.serial.disconnect() def set_serial_port(self, port): self.serial.set_port(port) def connected(self): return self.serial.connected def setUpdateInterval(seld, interval): pass def _startStatusReader(self): """ Starts the status readers. """ # Already started? if self.statusReaderIsAlive: return # Create thread and start reading the board status self.statusReaderIsAlive = True self.statusReaderThread = threading.Thread(target=self._statusReader) self.statusReaderThread.setDaemon = True self.statusReaderThread.start() def _stopStatusReader(self): """ Stops the status reader. """ if not self.statusReaderIsAlive: return self.statusReaderIsAlive = False #time.sleep(self.statusReaderUpdateInterval) self.statusReaderThread.join() def _statusReader(self): """ Sends status requests to the flight controller board. """ while self.statusReaderIsAlive: #self._logger.debug("Sending new status query") message = GetBoardStatusMessage() self.serial.writeMessage(message) #self.updateBoardSettings() time.sleep(self.statusReaderUpdateInterval) def _onMessageReceived(self): """ Callback method which is called when a new messages was received by the serial connection. """ try: messages = list(get_all_from_queue(self.serial.messageQueue)) #message = self.serial.messageQueue.get(False) #if message[0] is None: # return #if message[0].commandType == CommandTypes.GET_BOARD_STATUS: # self._updateStatus(message[0], message[1]) #elif message[0].commandType == CommandTypes.GET_BOARD_SETTINGS: # self._onBoardSettingsUpdated(message[0], message[1]) if len(messages) > 0: #self._logger.debug("New status response received") for message in messages: if message[0] is None: continue if message[0].commandType == CommandTypes.GET_BOARD_STATUS: self._updateStatus(message[0], message[1]) elif message[0].commandType == CommandTypes.GET_BOARD_SETTINGS: self._onBoardSettingsUpdated(message[0], message[1]) elif message[0].commandType == CommandTypes.DEBUG_INT_VALUES: self.boardStatus.updateFromMessage(message[0], message[1]) # Emit signal self.board_status_updated.emit() elif message[0].commandType == CommandTypes.DEBUG_FLOAT_VALUES: self.boardStatus.updateFromMessage(message[0], message[1]) # Emit signal self.board_status_updated.emit() elif message[0].commandType == CommandTypes.DEBUG_STRING_MESSAGE: self.boardStatus.updateFromMessage(message[0], message[1]) # Emit signal self.board_status_updated.emit() else: self.data_object_received.emit() except StopIteration: return def _updateStatus(self, message, timestamp): """ Updates the board status with the received data from the serial connection. """ self.boardStatus.updateFromMessage(message, timestamp) #self.boardStatus.printStatus() # Emit signal self.board_status_updated.emit() def _onBoardSettingsUpdated(self, message, timestamp): """Called when an board settings (0x0020) message was received. The board settings will be updated with the received message.""" self.fmuSettings.updateFromMessage(message, timestamp) # Emit signal self.board_settings_updated.emit() ########################################### ## Output channels (servo, engine) methods ########################################### def setServoPos(self, servo_nr=1, pos=0): """Moves the specified servo to the given position.""" message = ServoPositionMessage(servo_nr, pos) self.serial.writeMessage(message) def getServoPos(self, servo_nr=1): pass ########################################### ## Board settings methods ########################################### def updateBoardSettings(self): """Sends an update board settings message.""" message = GetBoardSettingsMessage() self.serial.writeMessage(message) def saveBoardSettings(self): """Saves all firmware settings to the flash memory.""" message = SaveSettingsMessage() self.serial.writeMessage(message) def setPIDRollCoefficients(self, p_fac, i_fac, d_fac, i_limit): message = SetRollPIDCoefficientsMessage(p_fac, i_fac, d_fac, i_limit) self.serial.writeMessage(message) def setPIDPitchCoefficients(self, p_fac, i_fac, d_fac, i_limit): message = SetPitchPIDCoefficientsMessage(p_fac, i_fac, d_fac, i_limit) self.serial.writeMessage(message) def setPIDYawCoefficients(self, p_fac, i_fac, d_fac, i_limit): message = SetYawPIDCoefficientsMessage(p_fac, i_fac, d_fac, i_limit) self.serial.writeMessage(message) def setDcmRollCoefficients(self, p_factor, i_factor): message = SetDcmRollCoefficientsMessage(p_factor, i_factor) self.serial.writeMessage(message) def setDcmPitchCoefficients(self, p_factor, i_factor): message = SetDcmPitchCoefficientsMessage(p_factor, i_factor) self.serial.writeMessage(message) def setDcmYawCoefficients(self, p_factor, i_factor): message = SetDcmYawCoefficientsMessage(p_factor, i_factor) self.serial.writeMessage(message)
while True: p = raw_input() if(p == 'q'): global serial serial.close() sys.exit(0) printAll = not printAll def logData(data): global dataBuffer dataBuffer.append(data) print data dataBuffer = [] serial = SerialConnection() serial.connect("/dev/tty.usbserial-FTG90JQK") printAll = False threading.Thread(target = getMode).start() threading.Thread(target = sender).start() while True: var = serial.readline() if printAll: logData(var) else: if var[:6] == "$GPGGA": parts = var.split(",") if(parts[2] != "" and parts[4] != ""): lat = float(parts[2]) if parts[3] == "S": lat = -lat long = float(parts[4])
print sys.argv try: fakePacket = False if len(sys.argv) == 2: fakePacket = True databuffer = [] isReady = False printNulls = True sendStars = True serial = SerialConnection() print "Serial created" serial.connect("/dev/ttyUSB0") print "Serial Connected..." socketConnection = SocketConnection() print "Socket created" if(socketConnection.connectAsSender()): threading.Thread(target = sendLoop).start() threading.Thread(target = getInput).start() print 'Thread started' while not isReady: time.sleep(0.1)