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)
