def __init__(self, controllerManager=None): super(StabilizationPage, self).__init__() if controllerManager is None: raise Exception self.fmuController = controllerManager self.fmuController.board_settings_updated.connect(self._onBoardSettingsUpdated) self._fmuSettings = FmuSettings() # Create the UI self._createUi()
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()
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)
class StabilizationPage(QtGui.QWidget): def __init__(self, controllerManager=None): super(StabilizationPage, self).__init__() if controllerManager is None: raise Exception self.fmuController = controllerManager self.fmuController.board_settings_updated.connect(self._onBoardSettingsUpdated) self._fmuSettings = FmuSettings() # Create the UI self._createUi() def _createUi(self): """Initializes the UI.""" mainLayout = QtGui.QVBoxLayout() # Stabilization Coefficients groupBox = QtGui.QGroupBox("Rate Stabilization (Inner Loop)") gridLayout = QtGui.QGridLayout() groupBox.setLayout(gridLayout) mainLayout.addWidget(groupBox) gridLayout.setSpacing(4) gridLayout.setColumnMinimumWidth(0, 20) gridLayout.setColumnMinimumWidth(1, 80) gridLayout.setColumnMinimumWidth(2, 20) gridLayout.setColumnMinimumWidth(3, 100) gridLayout.setColumnMinimumWidth(4, 100) gridLayout.setColumnMinimumWidth(5, 100) gridLayout.setColumnMinimumWidth(6, 100) gridLayout.setColumnMinimumWidth(7, 150) gridLayout.setColumnStretch(0, 0) gridLayout.setColumnStretch(1, 1) gridLayout.setColumnStretch(2, 0) gridLayout.setColumnStretch(3, 1) gridLayout.setColumnStretch(4, 1) gridLayout.setColumnStretch(5, 1) gridLayout.setColumnStretch(6, 1) gridLayout.setColumnStretch(7, 2) # Add spacer items spacer = QtGui.QSpacerItem(20, 20) gridLayout.addItem(spacer, 0, 0) spacer = QtGui.QSpacerItem(20, 20) gridLayout.addItem(spacer, 0, 2) spacer = QtGui.QSpacerItem(150, 20) gridLayout.addItem(spacer, 0, 7) spacer = QtGui.QSpacerItem(50, 120) gridLayout.addItem(spacer, 8, 0) # Headers header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("Proportional") gridLayout.addWidget(header, 0, 3) header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("Integral") gridLayout.addWidget(header, 0, 4) header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("Derivative") gridLayout.addWidget(header, 0, 5) header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("ILimit") gridLayout.addWidget(header, 0, 6) # Roll coefficients label = QtGui.QLabel("Roll") gridLayout.addWidget(label, 1, 1) self.spbRollKp = QtGui.QDoubleSpinBox() self.spbRollKp.setDecimals(5) self.spbRollKp.setMinimum(0) self.spbRollKp.setMaximum(5000) self.spbRollKp.setMinimumWidth(150) gridLayout.addWidget(self.spbRollKp, 1, 3) self.spbRollKi = QtGui.QDoubleSpinBox() self.spbRollKi.setDecimals(5) self.spbRollKi.setMinimum(0) self.spbRollKi.setMaximum(5000) self.spbRollKi.setMinimumWidth(150) gridLayout.addWidget(self.spbRollKi, 1, 4) self.spbRollKd = QtGui.QDoubleSpinBox() self.spbRollKd.setDecimals(5) self.spbRollKd.setMinimum(0) self.spbRollKd.setMaximum(5000) self.spbRollKd.setMinimumWidth(150) gridLayout.addWidget(self.spbRollKd, 1, 5) self.spbRollILimit = QtGui.QDoubleSpinBox() self.spbRollILimit.setDecimals(5) self.spbRollILimit.setMinimum(0) self.spbRollILimit.setMaximum(5000) self.spbRollILimit.setMinimumWidth(150) gridLayout.addWidget(self.spbRollILimit, 1, 6) # Pitch coefficients label = QtGui.QLabel("Pitch") gridLayout.addWidget(label, 2, 1) self.spbPitchKp = QtGui.QDoubleSpinBox() self.spbPitchKp.setDecimals(5) self.spbPitchKp.setMinimum(0) self.spbPitchKp.setMaximum(5000) self.spbPitchKp.setMinimumWidth(150) gridLayout.addWidget(self.spbPitchKp, 2, 3) self.spbPitchKi = QtGui.QDoubleSpinBox() self.spbPitchKi.setDecimals(5) self.spbPitchKi.setMinimum(0) self.spbPitchKi.setMaximum(5000) self.spbPitchKi.setMinimumWidth(150) gridLayout.addWidget(self.spbPitchKi, 2, 4) self.spbPitchKd = QtGui.QDoubleSpinBox() self.spbPitchKd.setDecimals(5) self.spbPitchKd.setMinimum(0) self.spbPitchKd.setMaximum(5000) self.spbPitchKd.setMinimumWidth(150) gridLayout.addWidget(self.spbPitchKd, 2, 5) self.spbILimitPitch = QtGui.QDoubleSpinBox() self.spbILimitPitch.setDecimals(5) self.spbILimitPitch.setMinimum(0) self.spbILimitPitch.setMaximum(5000) self.spbILimitPitch.setMinimumWidth(150) gridLayout.addWidget(self.spbILimitPitch, 2, 6) # Yaw coefficients label = QtGui.QLabel("Yaw") gridLayout.addWidget(label, 3, 1) self.spbYawKp = QtGui.QDoubleSpinBox() self.spbYawKp.setDecimals(5) self.spbYawKp.setMinimum(0) self.spbYawKp.setMaximum(5000) self.spbYawKp.setMinimumWidth(150) gridLayout.addWidget(self.spbYawKp, 3, 3) self.spbYawKi = QtGui.QDoubleSpinBox() self.spbYawKi.setDecimals(5) self.spbYawKi.setMinimum(0) self.spbYawKi.setMaximum(5000) self.spbYawKi.setMinimumWidth(150) gridLayout.addWidget(self.spbYawKi, 3, 4) self.spbYawKd = QtGui.QDoubleSpinBox() self.spbYawKd.setDecimals(5) self.spbYawKd.setMinimum(0) self.spbYawKd.setMaximum(5000) self.spbYawKd.setMinimumWidth(150) gridLayout.addWidget(self.spbYawKd, 3, 5) self.spbILimitYaw = QtGui.QDoubleSpinBox() self.spbILimitYaw.setDecimals(5) self.spbILimitYaw.setMinimum(0) self.spbILimitYaw.setMaximum(5000) self.spbILimitYaw.setMinimumWidth(150) gridLayout.addWidget(self.spbILimitYaw, 3, 6) # DCM Constants groupBox = QtGui.QGroupBox("Attitude Stabilization (Outer Loop)") gridLayout = QtGui.QGridLayout() groupBox.setLayout(gridLayout) gridLayout.setSpacing(4) mainLayout.addWidget(groupBox) gridLayout.setColumnMinimumWidth(0, 20) gridLayout.setColumnMinimumWidth(1, 80) gridLayout.setColumnMinimumWidth(2, 20) gridLayout.setColumnMinimumWidth(3, 100) gridLayout.setColumnMinimumWidth(4, 100) gridLayout.setColumnMinimumWidth(5, 150) gridLayout.setColumnStretch(0, 0) gridLayout.setColumnStretch(1, 1) gridLayout.setColumnStretch(2, 0) gridLayout.setColumnStretch(3, 1) gridLayout.setColumnStretch(4, 1) gridLayout.setColumnStretch(5, 2) # Add spacer items spacer = QtGui.QSpacerItem(20, 20) gridLayout.addItem(spacer, 0, 0) spacer = QtGui.QSpacerItem(20, 20) gridLayout.addItem(spacer, 0, 2) spacer = QtGui.QSpacerItem(150, 20) gridLayout.addItem(spacer, 0, 5) spacer = QtGui.QSpacerItem(50, 120) gridLayout.addItem(spacer, 3, 0) # Headers header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("Proportional") gridLayout.addWidget(header, 0, 3) header = HeadingLabel() header.setText("Integral") header.setTextAlign(QtCore.Qt.AlignHCenter) gridLayout.addWidget(header, 0, 4) # DCM Roll/Pitch label = QtGui.QLabel("Roll/Pitch") gridLayout.addWidget(label, 1, 1) self.spbDcmRollPitchPFactor = QtGui.QDoubleSpinBox() self.spbDcmRollPitchPFactor.setDecimals(5) self.spbDcmRollPitchPFactor.setMinimum(0) self.spbDcmRollPitchPFactor.setMaximum(5000) self.spbDcmRollPitchPFactor.setMinimumWidth(150) gridLayout.addWidget(self.spbDcmRollPitchPFactor, 1, 3) self.spbDcmRollPitchIFactor = QtGui.QDoubleSpinBox() self.spbDcmRollPitchIFactor.setDecimals(5) self.spbDcmRollPitchIFactor.setMinimum(0) self.spbDcmRollPitchIFactor.setMaximum(5000) self.spbDcmRollPitchIFactor.setMinimumWidth(150) gridLayout.addWidget(self.spbDcmRollPitchIFactor, 1, 4) # DCM Yaw label = QtGui.QLabel("Yaw") gridLayout.addWidget(label, 2, 1) self.spbDcmYawPFactor = QtGui.QDoubleSpinBox() self.spbDcmYawPFactor.setDecimals(5) self.spbDcmYawPFactor.setMinimum(0) self.spbDcmYawPFactor.setMaximum(5000) self.spbDcmYawPFactor.setMinimumWidth(150) gridLayout.addWidget(self.spbDcmYawPFactor, 2, 3) self.spbDcmYawIFactor = QtGui.QDoubleSpinBox() self.spbDcmYawIFactor.setDecimals(5) self.spbDcmYawIFactor.setMinimum(0) self.spbDcmYawIFactor.setMaximum(5000) self.spbDcmYawIFactor.setMinimumWidth(150) gridLayout.addWidget(self.spbDcmYawIFactor, 2, 4) # Stick range and limits groupBox = QtGui.QGroupBox("Stick ranges and limits") gridLayout = QtGui.QGridLayout() groupBox.setLayout(gridLayout) gridLayout.setSpacing(4) mainLayout.addWidget(groupBox) gridLayout.setColumnMinimumWidth(0, 20) gridLayout.setColumnMinimumWidth(1, 80) gridLayout.setColumnMinimumWidth(2, 20) gridLayout.setColumnMinimumWidth(3, 100) gridLayout.setColumnMinimumWidth(4, 100) gridLayout.setColumnMinimumWidth(5, 100) gridLayout.setColumnMinimumWidth(6, 150) gridLayout.setColumnStretch(0, 0) gridLayout.setColumnStretch(1, 1) gridLayout.setColumnStretch(2, 0) gridLayout.setColumnStretch(3, 1) gridLayout.setColumnStretch(4, 1) gridLayout.setColumnStretch(5, 1) gridLayout.setColumnStretch(6, 2) # Add spacer items spacer = QtGui.QSpacerItem(20, 20) gridLayout.addItem(spacer, 0, 0) spacer = QtGui.QSpacerItem(20, 20) gridLayout.addItem(spacer, 0, 2) spacer = QtGui.QSpacerItem(150, 20) gridLayout.addItem(spacer, 0, 6) spacer = QtGui.QSpacerItem(50, 120) gridLayout.addItem(spacer, 4, 0) # Headers header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("Roll") gridLayout.addWidget(header, 0, 3) header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("Pitch") gridLayout.addWidget(header, 0, 4) header = HeadingLabel() header.setTextAlign(QtCore.Qt.AlignHCenter) header.setText("Yaw") gridLayout.addWidget(header, 0, 5) # Full stick angle label = QtGui.QLabel("Full Stick Angle (deg)") gridLayout.addWidget(label, 1, 1) self.spbStickAngleRoll = QtGui.QDoubleSpinBox() self.spbStickAngleRoll.setMinimumWidth(150) gridLayout.addWidget(self.spbStickAngleRoll, 1, 3) self.spbStickAnglePitch = QtGui.QDoubleSpinBox() self.spbStickAnglePitch.setMinimumWidth(150) gridLayout.addWidget(self.spbStickAnglePitch, 1, 4) self.spbStickAngleYaw = QtGui.QDoubleSpinBox() self.spbStickAngleYaw.setMinimumWidth(150) gridLayout.addWidget(self.spbStickAngleYaw, 1, 5) # Full stick rate label = QtGui.QLabel("Full Stick Rate (deg/s)") gridLayout.addWidget(label, 2, 1) self.spbStickRateRoll = QtGui.QDoubleSpinBox() self.spbStickRateRoll.setMinimumWidth(150) gridLayout.addWidget(self.spbStickRateRoll, 2, 3) self.spbStickRatePitch = QtGui.QDoubleSpinBox() self.spbStickRatePitch.setMinimumWidth(150) gridLayout.addWidget(self.spbStickRatePitch, 2, 4) self.spbStickRateYaw = QtGui.QDoubleSpinBox() self.spbStickRateYaw.setMinimumWidth(150) gridLayout.addWidget(self.spbStickRateYaw, 2, 5) # Maximum rate in attitude mode label = QtGui.QLabel("Maximum Rate in Attitude Mode (deg/s)") gridLayout.addWidget(label, 3, 1) self.spbStickMaxRateAttRoll = QtGui.QDoubleSpinBox() self.spbStickMaxRateAttRoll.setMinimumWidth(150) gridLayout.addWidget(self.spbStickMaxRateAttRoll, 3, 3) self.spbStickMaxRateAttPitch = QtGui.QDoubleSpinBox() self.spbStickMaxRateAttPitch.setMinimumWidth(150) gridLayout.addWidget(self.spbStickMaxRateAttPitch, 3, 4) self.spbStickMaxRateAttYaw = QtGui.QDoubleSpinBox() self.spbStickMaxRateAttYaw.setMinimumWidth(150) gridLayout.addWidget(self.spbStickMaxRateAttYaw, 3, 5) mainLayout.addStretch() # Bottom buttons hLayout = QtGui.QHBoxLayout() self.bRestoreFactory = QtGui.QPushButton("Restore Factory Settings") self.bRestoreFactory.clicked.connect(self._onbRestoreFactoryClicked) hLayout.addWidget(self.bRestoreFactory) self.bReloadFromBoard = QtGui.QPushButton("Reload data from board") self.bReloadFromBoard.clicked.connect(self._onbReloadFromBoard) hLayout.addWidget(self.bReloadFromBoard) hLayout.addStretch() self.saveButton = QtGui.QPushButton("Save") self.saveButton.setEnabled(False) self.saveButton.clicked.connect(self._onSaveButtonClicked) hLayout.addWidget(self.saveButton) self.applyButton = QtGui.QPushButton("Apply") self.applyButton.setEnabled(False) self.applyButton.clicked.connect(self._onApplyButtonClicked) hLayout.addWidget(self.applyButton) mainLayout.addLayout(hLayout) # Set main layout self.setLayout(mainLayout) # Connect all signals self._connectSignals() def _connectSignals(self): self.spbRollKp.valueChanged.connect(self._onPidRollKpChanged) self.spbRollKi.valueChanged.connect(self._onPidRollKiChanged) self.spbRollKd.valueChanged.connect(self._onPidRollKdChanged) self.spbRollILimit.valueChanged.connect(self._onPidRollILimitChanged) self.spbPitchKp.valueChanged.connect(self._onValueChanged) self.spbPitchKi.valueChanged.connect(self._onValueChanged) self.spbPitchKd.valueChanged.connect(self._onValueChanged) self.spbILimitPitch.valueChanged.connect(self._onValueChanged) self.spbYawKp.valueChanged.connect(self._onValueChanged) self.spbYawKi.valueChanged.connect(self._onValueChanged) self.spbYawKd.valueChanged.connect(self._onValueChanged) self.spbILimitYaw.valueChanged.connect(self._onValueChanged) self.spbDcmRollPitchPFactor.valueChanged.connect(self._onDcmRollPitchPFactorChanged) self.spbDcmRollPitchIFactor.valueChanged.connect(self._onDcmRollPitchIFactorChanged) self.spbDcmYawPFactor.valueChanged.connect(self._onValueChanged) self.spbDcmYawIFactor.valueChanged.connect(self._onValueChanged) def _disconnectSignals(self): self.spbRollKp.valueChanged.disconnect(self._onPidRollKpChanged) self.spbRollKi.valueChanged.disconnect(self._onPidRollKiChanged) self.spbRollKd.valueChanged.disconnect(self._onPidRollKdChanged) self.spbRollILimit.valueChanged.disconnect(self._onPidRollILimitChanged) self.spbPitchKp.valueChanged.disconnect(self._onValueChanged) self.spbPitchKi.valueChanged.disconnect(self._onValueChanged) self.spbPitchKd.valueChanged.disconnect(self._onValueChanged) self.spbILimitPitch.valueChanged.disconnect(self._onValueChanged) self.spbYawKp.valueChanged.disconnect(self._onValueChanged) self.spbYawKi.valueChanged.disconnect(self._onValueChanged) self.spbYawKd.valueChanged.disconnect(self._onValueChanged) self.spbILimitYaw.valueChanged.disconnect(self._onValueChanged) self.spbDcmRollPitchPFactor.valueChanged.disconnect(self._onDcmRollPitchPFactorChanged) self.spbDcmRollPitchIFactor.valueChanged.disconnect(self._onDcmRollPitchIFactorChanged) self.spbDcmYawPFactor.valueChanged.disconnect(self._onValueChanged) self.spbDcmYawIFactor.valueChanged.disconnect(self._onValueChanged) def _onApplyButtonClicked(self): # Send settings to the controller self._applyValues() # Disable apply button self.applyButton.setEnabled(False) def _applyValues(self): """Writes all settings values to the controller.""" # PID Roll settings self.fmuController.setPIDRollCoefficients(self.spbRollKp.value(), self.spbRollKi.value(), self.spbRollKd.value(), self.spbRollILimit.value()) # PID Pitch settings self.fmuController.setPIDPitchCoefficients(self.spbPitchKp.value(), self.spbPitchKi.value(), self.spbPitchKd.value(), self.spbILimitPitch.value()) # PID Yaw settings self.fmuController.setPIDYawCoefficients(self.spbYawKp.value(), self.spbYawKi.value(), self.spbYawKd.value(), self.spbILimitYaw.value()) # DCM Roll settings self.fmuController.setDcmRollCoefficients(self.spbDcmRollPitchPFactor.value(), self.spbDcmRollPitchIFactor.value()) # DCM Pitch settings self.fmuController.setDcmPitchCoefficients(self.spbDcmRollPitchPFactor.value(), self.spbDcmRollPitchIFactor.value()) # DCM Yaw settings self.fmuController.setDcmYawCoefficients(self.spbDcmYawPFactor.value(), self.spbDcmYawIFactor.value()) # Disable apply button self.applyButton.setEnabled(False) def _onSaveButtonClicked(self): # Send settings to the controller self._applyValues() # Save settings self._saveValues() # Disable save and apply button self.saveButton.setEnabled(False) self.applyButton.setEnabled(False) def _saveValues(self): """Saves the settings on the controller.""" # Save settings self.fmuController.saveBoardSettings() def _onBoardSettingsUpdated(self): """Called when the board settings are updated.""" self._updateValues() def _updateValues(self): """Update GUI elements.""" # Disable events self._disconnectSignals() self.spbRollKp.setValue(self._fmuSettings.pidRollPFactor) self.spbRollKi.setValue(self._fmuSettings.pidRollIFactor) self.spbRollKd.setValue(self._fmuSettings.pidRollDFactor) self.spbRollILimit.setValue(self._fmuSettings.pidRollILimit) self.spbPitchKp.setValue(self._fmuSettings.pidPitchPFactor) self.spbPitchKi.setValue(self._fmuSettings.pidPitchIFactor) self.spbPitchKd.setValue(self._fmuSettings.pidPitchDFactor) self.spbILimitPitch.setValue(self._fmuSettings.pidPitchILimit) self.spbYawKp.setValue(self._fmuSettings.pidYawPFactor) self.spbYawKi.setValue(self._fmuSettings.pidYawIFactor) self.spbYawKd.setValue(self._fmuSettings.pidYawDFactor) self.spbILimitYaw.setValue(self._fmuSettings.pidYawILimit) self.spbDcmRollPitchPFactor.setValue(self._fmuSettings.dcmRollPFactor) self.spbDcmRollPitchIFactor.setValue(self._fmuSettings.dcmRollIFactor) self.spbDcmYawPFactor.setValue(self._fmuSettings.dcmYawPFactor) self.spbDcmYawIFactor.setValue(self._fmuSettings.dcmYawIFactor) # Re-enable events self._connectSignals() def _onValueChanged(self, value): self._enableButtons() def _onPidRollKpChanged(self, value): self._enableButtons() def _onPidRollKiChanged(self, value): self._enableButtons() def _onPidRollKdChanged(self, value): self._enableButtons() def _onPidRollILimitChanged(self, value): self._enableButtons() def _onDcmRollPitchPFactorChanged(self, value): self._enableButtons() def _onDcmRollPitchIFactorChanged(self, value): self._enableButtons() def _onKalmanRollRAngleChanged(self, value): self._enableButtons() def _enableButtons(self): # Settings were changed; enable save and apply buttons self.saveButton.setEnabled(True) self.applyButton.setEnabled(True) def _onbRestoreFactoryClicked(self): self._fmuSettings.restoreStabilizationFactorySettings() self._updateValues() self._enableButtons() def _onbReloadFromBoard(self): self.fmuController.updateBoardSettings()