diff --git a/rosplane_tuning/CMakeLists.txt b/rosplane_tuning/CMakeLists.txt index 8d9020e..1915ef4 100644 --- a/rosplane_tuning/CMakeLists.txt +++ b/rosplane_tuning/CMakeLists.txt @@ -57,29 +57,6 @@ install(TARGETS #### END OF EXECUTABLES ### -#### RQT #### - -ament_python_install_package(rqt_tuning_gui - PACKAGE_DIR src/rqt_tuning_gui -) - -install(FILES - plugin.xml - DESTINATION share/${PROJECT_NAME} -) - -install(DIRECTORY - resource - DESTINATION share/${PROJECT_NAME} -) - -install(PROGRAMS - scripts/rqt_tuning_gui - DESTINATION lib/${PROJECT_NAME} -) - -#### END OF RQT #### - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/rosplane_tuning/launch/rosplane_tuning_gui.launch.py b/rosplane_tuning/launch/rosplane_tuning_gui.launch.py deleted file mode 100644 index 3f75d0b..0000000 --- a/rosplane_tuning/launch/rosplane_tuning_gui.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -import os -import sys -from launch import LaunchDescription -from launch.descriptions import executable -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - # Create the package directory - rosplane_dir = get_package_share_directory('rosplane') - - return LaunchDescription([ - Node( - package='rosplane_tuning', - executable='tuning_gui.py', - name='tuning_gui_node', - output = 'screen' - ) - ]) diff --git a/rosplane_tuning/plugin.xml b/rosplane_tuning/plugin.xml deleted file mode 100644 index 38dd565..0000000 --- a/rosplane_tuning/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - Tuning GUI for ROSflight - - - - - - - system-help - Super helpful GUI for tuning ROSplane. - - - diff --git a/rosplane_tuning/resource/tuning_gui.ui b/rosplane_tuning/resource/tuning_gui.ui deleted file mode 100644 index 120639b..0000000 --- a/rosplane_tuning/resource/tuning_gui.ui +++ /dev/null @@ -1,869 +0,0 @@ - - - ROSPlaneTuningGUI - - - - 0 - 0 - 995 - 856 - - - - - 0 - 0 - - - - - 810 - 610 - - - - MainWindow - - - - - 20 - 20 - 810 - 631 - - - - - 0 - 0 - - - - - 800 - 600 - - - - - 16777215 - 16777215 - - - - - - -1 - -1 - 811 - 631 - - - - - - - Qt::Horizontal - - - QSizePolicy::Maximum - - - - 10 - 20 - - - - - - - - QFrame::Plain - - - 2 - - - Qt::Horizontal - - - - - - - - - - 16777215 - 50 - - - - <html><head/><body><p align="center"></p><p align="center"><span style=" font-size:16pt;">ROSplane Tuning GUI</span></p></body></html> - - - - - - - - 150 - 0 - - - - - 200 - 16777215 - - - - false - - - QFrame::Box - - - QFrame::Plain - - - 2 - - - 0 - - - RC Override Active - - - Qt::AlignCenter - - - - - - - - - QLayout::SetFixedSize - - - - - 6 - - - QLayout::SetDefaultConstraint - - - 0 - - - - - Qt::Horizontal - - - QSizePolicy::Maximum - - - - 10 - 20 - - - - - - - - 0 - - - QLayout::SetDefaultConstraint - - - 10 - - - 10 - - - - - - 16777215 - 40 - - - - Mode Select - - - - - - - - 16777215 - 10 - - - - QFrame::Plain - - - 1 - - - Qt::Horizontal - - - - - - - Course - - - - - - - Roll - - - - - - - Pitch - - - - - - - Airspeed - - - - - - - Altitude - - - - - - - Run - - - - - - - Undo - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - Save - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - Clear - - - - - - - - - QLayout::SetDefaultConstraint - - - 0 - - - 0 - - - - - - - - - - 70 - 16777215 - - - - <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">p </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 0 - - - - - - - - - - QFrame::Plain - - - 1 - - - Qt::Vertical - - - - - - - 10 - - - - - Previous Value: - - - - - - - Saved Value: - - - - - - - - - - - - 0 - 0 - - - - - 16777215 - 40 - - - - Qt::Horizontal - - - QSlider::TicksBelow - - - 0 - - - - - - - Qt::Vertical - - - QSizePolicy::Maximum - - - - 20 - 100 - - - - - - - - - - - - - - - 70 - 16777215 - - - - <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">i </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 0 - - - - - - - - - - QFrame::Plain - - - 1 - - - Qt::Vertical - - - - - - - 10 - - - - - Previous Value: - - - - - - - Saved Value: - - - - - - - - - - - - 0 - 0 - - - - - 16777215 - 40 - - - - Qt::Horizontal - - - QSlider::TicksBelow - - - - - - - Qt::Vertical - - - QSizePolicy::Maximum - - - - 20 - 100 - - - - - - - - - - - - - - - 70 - 16777215 - - - - <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">d</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;"/><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 0 - - - - - - - - - - QFrame::Plain - - - 1 - - - Qt::Vertical - - - - - - - 10 - - - - - Previous Value: - - - - - - - Saved Value: - - - - - - - - - - - - 0 - 0 - - - - - 16777215 - 40 - - - - Qt::Horizontal - - - QSlider::TicksBelow - - - - - - - Qt::Vertical - - - QSizePolicy::Maximum - - - - 20 - 100 - - - - - - - - - - - - - - - - 0 - - - - - - 100 - 0 - - - - - 16777215 - 60 - - - - Current -Parameter -Values - - - Qt::AlignCenter - - - - - - - - 16777215 - 20 - - - - false - - - background-color: #b3b3b3 - - - QFrame::NoFrame - - - COURSE: - - - Qt::AlignCenter - - - - - - - - 16777215 - 65 - - - - c_kp: 0.0 -c_ki: 0.0 -c_kd: 0.0 - - - - - - - - 16777215 - 20 - - - - background-color: #b3b3b3 - - - ROLL: - - - Qt::AlignCenter - - - - - - - - 16777215 - 65 - - - - r_kp: 0.0 -r_ki: 0.0 -r_kd: 0.0 - - - - - - - - 16777215 - 20 - - - - background-color: #b3b3b3 - - - PITCH: - - - Qt::AlignCenter - - - - - - - - 16777215 - 65 - - - - p_kp: 0.0 -p_ki: 0.0 -p_kd: 0.0 - - - - - - - - 16777215 - 20 - - - - background-color: #b3b3b3 - - - AIRSPEED: - - - Qt::AlignCenter - - - - - - - - 16777215 - 65 - - - - a_t_kp: 0.0 -a_t_ki: 0.0 -a_t_kd: 0.0 - - - - - - - - 16777215 - 20 - - - - background-color: #b3b3b3 - - - ALTITUDE: - - - Qt::AlignCenter - - - - - - - - 16777215 - 65 - - - - a_kp: 0.0 -a_ki: 0.0 -a_kd: 0.0 - - - - - - - - - QFrame::Plain - - - 2 - - - Qt::Vertical - - - - - - - - - - - - diff --git a/rosplane_tuning/scripts/rqt_tuning_gui b/rosplane_tuning/scripts/rqt_tuning_gui deleted file mode 100644 index 4987816..0000000 --- a/rosplane_tuning/scripts/rqt_tuning_gui +++ /dev/null @@ -1,9 +0,0 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages = ['rqt_tuning_gui'], - package_dir = {'': 'src'}, -) - -setup(**d) diff --git a/rosplane_tuning/src/rqt_tuning_gui/__init__.py b/rosplane_tuning/src/rqt_tuning_gui/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py deleted file mode 100644 index 36985e5..0000000 --- a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py +++ /dev/null @@ -1,531 +0,0 @@ -import os -import rclpy -from rclpy import qos -from rclpy.node import Node -from rcl_interfaces.srv import SetParameters, GetParameters -from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType, ParameterEvent - -from rosflight_msgs.msg import Status - -from qt_gui.plugin import Plugin -from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QMainWindow, QWidget, QSizePolicy, QHBoxLayout, QVBoxLayout, QRadioButton, QPushButton, QLabel, QLayout, QSlider, QDoubleSpinBox -from python_qt_binding import QtCore, QtGui - -from ament_index_python import get_resource - -class ParameterClient(Node): - def __init__(self): - super().__init__('tuning_gui_parameter_client') - - self.get_client = self.create_client(GetParameters, '/autopilot/get_parameters') - while not self.get_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('GET_PARAMETERS service not available, waiting again... is ROSplane autopilot running?') - self.get_req = GetParameters.Request() - - self.set_client = self.create_client(SetParameters, '/autopilot/set_parameters') - while not self.get_client.wait_for_service(timeout_sec=1.0): - self.get_logger().info('SET_PARAMETERS service not available, waiting again... is ROSplane autopilot running?') - self.set_req = SetParameters.Request() - - def get_autopilot_params(self, param_names:list[str]): - self.get_req.names = param_names - self.get_future = self.get_client.call_async(self.get_req) - rclpy.spin_until_future_complete(self, self.get_future) - return self.get_future.result() - - def set_autopilot_params(self, param_names:list[str], param_values:list[float]): - assert len(param_values) == len(param_names) - # Create list of Parameter objects - parameters = [] - for i in range(len(param_names)): - newParamValue = ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=param_values[i]) - parameters.append(Parameter(name=param_names[i], value=newParamValue)) - self.set_req.parameters = parameters - self.set_future = self.set_client.call_async(self.set_req) - rclpy.spin_until_future_complete(self, self.set_future) - return self.set_future.result() - - -class ROSflightGUI(Plugin): - colorChanged = QtCore.pyqtSignal(QtGui.QColor) - - def __init__(self, context): - super(ROSflightGUI, self).__init__(context) - # Give QObjects reasonable names - self.setObjectName('ROSflightGUI') - self._context = context - - # Process standalone plugin command-line arguments - from argparse import ArgumentParser - parser = ArgumentParser() - # Add argument(s) to the parser. - parser.add_argument("-q", "--quiet", action="store_true", - dest="quiet", - help="Put plugin in silent mode") - args, unknowns = parser.parse_known_args(context.argv()) - if not args.quiet: - print('arguments: ', args) - print('unknowns: ', unknowns) - - # Create QWidget - self._widget = QMainWindow() - # Get path to UI file which should be in the "resource" folder of this package - _, path = get_resource('packages', 'rosplane_tuning') - ui_file = os.path.join(path, 'share', 'rosplane_tuning', 'resource', 'tuning_gui.ui') - # Extend the widget with all attributes and children from UI file - loadUi(ui_file, self._widget) - # Give QObjects reasonable names - self._widget.setObjectName('ROSflightTuningUi') - # Show _widget.windowTitle on left-top of each plugin (when - # it's set in _widget). This is useful when you open multiple - # plugins at once. Also if you open multiple instances of your - # plugin at once, these lines add number to make it easy to - # tell from pane to pane. - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - # Add widget to the user interface - context.add_widget(self._widget) - - # Create ROS2 node to do node things - self._node = ParameterClient() - self.rc_override = False - self._rc_override_sub = self._context.node.create_subscription(Status, '/status', self.status_sub_callback, qos.qos_profile_sensor_data) - self._param_sub = self._context.node.create_subscription(ParameterEvent, '/parameter_events', self.parameter_event_callback, 10) - - # Set up the UI - self.setup_UI() - - - def setup_UI(self): - self.initialize_gui() - self.connectSignalSlots() - self.set_sizes() - - def initialize_gui(self): - self.tuning_mode = '' - self.curr_kp = 0.0 - self.curr_kd = 0.0 - self.curr_ki = 0.0 - - self.temp_kp = 0.0 - self.temp_kd = 0.0 - self.temp_ki = 0.0 - - self.undo_kp = self.curr_kp - self.undo_kd = self.curr_kd - self.undo_ki = self.curr_ki - # This allows us to have different ranges for fine tuning kp, ki, and kd - self.kp_edit_dist = 2.0 - self.ki_edit_dist = 0.5 - self.kd_edit_dist = 2.0 - # Boolean values for controlling debugging statements - self.time = False - self.disp = True - - # Original parameters saved at init, called with clear button - self.call_originals() - - # Strings for the readout list - self.ckp = self.orig_c_kp - self.cki = self.orig_c_ki - self.ckd = self.orig_c_kd - self.rkp = self.orig_r_kp - self.rki = self.orig_r_ki - self.rkd = self.orig_r_kd - self.pkp = self.orig_p_kp - self.pki = self.orig_p_ki - self.pkd = self.orig_p_kd - self.akp = self.orig_a_kp - self.aki = self.orig_a_ki - self.akd = self.orig_a_kd - self.a_t_kp = self.orig_a_t_kp - self.a_t_ki = self.orig_a_t_ki - self.a_t_kd = self.orig_a_t_kd - - def call_originals(self): - (self.orig_c_kp, self.orig_c_ki, self.orig_c_kd) = self.get_params(['c_kp', 'c_ki', 'c_kd']) - (self.orig_p_kp, self.orig_p_ki, self.orig_p_kd) = self.get_params(['p_kp', 'p_ki', 'p_kd']) - (self.orig_r_kp, self.orig_r_ki, self.orig_r_kd) = self.get_params(['r_kp', 'r_ki', 'r_kd']) - (self.orig_a_t_kp, self.orig_a_t_ki, self.orig_a_t_kd) = self.get_params(['a_t_kp', 'a_t_ki', 'a_t_kd']) - (self.orig_a_kp, self.orig_a_ki, self.orig_a_kd) = self.get_params(['a_kp', 'a_ki', 'a_kd']) - - - def set_sizes(self): - self._widget.kpSlider.setMinimum(-100) - self._widget.kpSlider.setMaximum(100) - self._widget.kiSlider.setMinimum(-100) - self._widget.kiSlider.setMaximum(100) - self._widget.kdSlider.setMinimum(-100) - self._widget.kdSlider.setMaximum(100) - self._widget.kpSpinBox.setSingleStep(0.001) - self._widget.kiSpinBox.setSingleStep(0.001) - self._widget.kdSpinBox.setSingleStep(0.001) - self._widget.kpSpinBox.setDecimals(3) - self._widget.kiSpinBox.setDecimals(3) - self._widget.kdSpinBox.setDecimals(3) - - def shutdown_plugin(self): - # TODO unregister all publishers here - pass - - def save_settings(self, plugin_settings, instance_settings): - # TODO save intrinsic configuration, usually using: - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # TODO restore intrinsic configuration, usually using: - # v = instance_settings.value(k) - pass - - #def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure - # This will enable a setting button (gear icon) in each dock widget title bar - # Usually used to open a modal configuration dialog - - def connectSignalSlots(self): - # This is where we define signal slots (callbacks) for when the buttons get clicked - self._widget.CourseButton.toggled.connect(self.courseButtonCallback) - self._widget.pitchButton.toggled.connect(self.pitchButtonCallback) - self._widget.rollButton.toggled.connect(self.rollButtonCallback) - self._widget.airspeedButton.toggled.connect(self.airspeedButtonCallback) - self._widget.altitudeButton.toggled.connect(self.altitudeButtonCallback) - self._widget.runButton.clicked.connect(self.runButtonCallback) - self._widget.clearButton.clicked.connect(self.clearButtonCallback) - self._widget.saveButton.clicked.connect(self.saveButtonCallback) - self._widget.undoButton.clicked.connect(self.undoButtonCallback) - self._widget.kpSlider.valueChanged.connect(self.kp_slider_callback) - self._widget.kiSlider.valueChanged.connect(self.ki_slider_callback) - self._widget.kdSlider.valueChanged.connect(self.kd_slider_callback) - self._widget.kpSpinBox.valueChanged.connect(self.kpSpinBox_callback) - self._widget.kiSpinBox.valueChanged.connect(self.kiSpinBox_callback) - self._widget.kdSpinBox.valueChanged.connect(self.kdSpinBox_callback) - # Color changer for the RC Override box - self.colorChanged.connect(self.on_color_change) - - def courseButtonCallback(self): - if self._widget.CourseButton.isChecked(): - if self.disp: print("COURSE gains selected") - # Set the tuning mode - self.tuning_mode = 'c' - # Get the other parameters from ROS - (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) - # Set the sliders to the appropriate values - self.set_sliders() - self.set_SpinBoxes() - self.set_previousVals() - - def rollButtonCallback(self): - if self._widget.rollButton.isChecked(): - if self.disp: print("ROLL gains selected") - # Set the tuning mode - self.tuning_mode = 'r' - # Get the other parameters from ROS - (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) - self.set_sliders() - self.set_SpinBoxes() - self.set_previousVals() - - def pitchButtonCallback(self): - if self._widget.pitchButton.isChecked(): - if self.disp: print("PITCH gains selected") - # Set the tuning mode - self.tuning_mode = 'p' - # Get the other parameters from ROS - (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) - self.set_sliders() - self.set_SpinBoxes() - self.set_previousVals() - - def airspeedButtonCallback(self): - if self._widget.airspeedButton.isChecked(): - if self.disp: print("AIRSPEED gains selected") - # Set the tuning mode - self.tuning_mode = 'a_t' - # Get the other parameters from ROS - (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) - self.set_sliders() - self.set_SpinBoxes() - self.set_previousVals() - - def altitudeButtonCallback(self): - if self._widget.altitudeButton.isChecked(): - if self.disp: print("ALTITUDE gains selected") - # Set the tuning mode - self.tuning_mode = 'a' - # Get the other parameters from ROS - (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) - self.set_sliders() - self.set_SpinBoxes() - self.set_previousVals() - - def get_params(self, param_names:list[str]): - response = self._node.get_autopilot_params(param_names) - return [par_val.double_value for par_val in response.values] - - def set_sliders(self): - # Sliders have an integer range. Set this from +- 100 - self._widget.kpSlider.setValue(0) - self._widget.kiSlider.setValue(0) - self._widget.kdSlider.setValue(0) - - def set_SpinBoxes(self): - # Sliders have an integer range. Set this from +- 100 - self._widget.kpSpinBox.setMinimum(self.curr_kp - self.kp_edit_dist) - self._widget.kpSpinBox.setMaximum(self.curr_kp + self.kp_edit_dist) - self._widget.kpSpinBox.setValue(self.curr_kp) - - self._widget.kiSpinBox.setMinimum(self.curr_ki - self.ki_edit_dist) - self._widget.kiSpinBox.setMaximum(self.curr_ki + self.ki_edit_dist) - self._widget.kiSpinBox.setValue(self.curr_ki) - - self._widget.kdSpinBox.setMinimum(self.curr_kd - self.kd_edit_dist) - self._widget.kdSpinBox.setMaximum(self.curr_kd + self.kd_edit_dist) - self._widget.kdSpinBox.setValue(self.curr_kd) - - def kp_slider_callback(self): - slider_val = self._widget.kpSlider.value() - self.temp_kp = self.curr_kp + self.kp_edit_dist * slider_val / 100 - # if self.disp: print(self.temp_kp) - self._widget.kpSpinBox.setValue(self.temp_kp) - - def ki_slider_callback(self): - slider_val = self._widget.kiSlider.value() - self.temp_ki = self.curr_ki + self.ki_edit_dist * slider_val / 100 - # if self.disp: print(self.temp_ki) - self._widget.kiSpinBox.setValue(self.temp_ki) - - def kd_slider_callback(self): - slider_val = self._widget.kdSlider.value() - self.temp_kd = self.curr_kd + self.kd_edit_dist * slider_val / 100 - # if self.disp: print(self.temp_kd) - self._widget.kdSpinBox.setValue(self.temp_kd) - - def kpSpinBox_callback(self): - kpSpinBox_value = self._widget.kpSpinBox.value() - self.temp_kp = kpSpinBox_value - # slider_val = (self.temp_kp - self.curr_kp)*100/self.kp_edit_dist - # self._widget.kpSlider.setValue(int(slider_val)) - - def kiSpinBox_callback(self): - kiSpinBox_value = self._widget.kiSpinBox.value() - self.temp_ki = kiSpinBox_value - # slider_val = (self.temp_ki - self.curr_ki)*100/self.ki_edit_dist - # self._widget.kiSlider.setValue(int(slider_val)) - - def kdSpinBox_callback(self): - kdSpinBox_value = self._widget.kdSpinBox.value() - self.temp_kd = kdSpinBox_value - # slider_val = (self.temp_kd - self.curr_kd)*100/self.kd_edit_dist - # self._widget.kdSlider.setValue(int(slider_val)) - - def runButtonCallback(self, *args, mode=None): - # call this if run button is pushed - # Set the undo values to be the current values - self.undo_kp = self.curr_kp - self.undo_ki = self.curr_ki - self.undo_kd = self.curr_kd - # Set current variables to be temp variables - self.curr_kp = self.temp_kp - self.curr_ki = self.temp_ki - self.curr_kd = self.temp_kd - #execute ros param set functions - if mode is not None: - tun_mode = mode - else: - tun_mode = self.tuning_mode - - if self.disp: print('RUNNING parameters') - - param_names = [tun_mode+'_kp', tun_mode+'_ki', tun_mode+'_kd'] - print(param_names) - param_vals = [self.curr_kp, self.curr_ki, self.curr_kd] - result = self._node.set_autopilot_params(param_names, param_vals) - - for msg in result.results: - if not msg.successful: - print(f'WARNING: {msg} was not successful') - - if self.disp: - print('Kp set to:', self.curr_kp) - print('Ki set to:', self.curr_ki) - print('Kd set to:', self.curr_kd) - - # Reinitialize the gui - self.set_sliders() - self.set_SpinBoxes() - self.set_previousVals() - - # Resets the current mode's inputs to original or last save values - def clearButtonCallback(self): - # for mode in ['c', 'p', 'r', 'a', 'a_t']: - # if self.disp: print('\nCLEARING <' + mode + '> parameters') - # params = ['p','i','d'] - # for param in params: - # orig_var_name = f"orig_{mode}_k{param}" - # #get parameter values for orig_var_name - # original_value = getattr(self,orig_var_name) - # #generate curr param variable names - # curr_var_name = f"temp_k{param}" - # #Assign original values to curr parameters - # setattr(self, curr_var_name, float(original_value)) - # print(f'{curr_var_name} set to {original_value}') - # #run button callback to apply changes - # self.tuning_mode = mode - # self.runButtonCallback() - # reset current values to prevent the undos from being wild - self.curr_kd = 0.0 - self.curr_ki = 0.0 - self.curr_kp = 0.0 - - temporary_kp = 0.0 - temporary_ki = 0.0 - temporary_kd = 0.0 - - self.temp_kp = self.orig_c_kp - self.temp_ki = self.orig_c_ki - self.temp_kd = self.orig_c_kd - if self.tuning_mode == 'c': - temporary_kp = self.temp_kp - temporary_ki = self.temp_ki - temporary_kd = self.temp_kd - self.runButtonCallback(mode='c') - - self.temp_kp = self.orig_r_kp - self.temp_ki = self.orig_r_ki - self.temp_kd = self.orig_r_kd - if self.tuning_mode == 'r': - temporary_kp = self.temp_kp - temporary_ki = self.temp_ki - temporary_kd = self.temp_kd - self.runButtonCallback(mode='r') - - self.temp_kp = self.orig_p_kp - self.temp_ki = self.orig_p_ki - self.temp_kd = self.orig_p_kd - if self.tuning_mode == 'p': - temporary_kp = self.temp_kp - temporary_ki = self.temp_ki - temporary_kd = self.temp_kd - self.runButtonCallback(mode='p') - - self.temp_kp = self.orig_a_kp - self.temp_ki = self.orig_a_ki - self.temp_kd = self.orig_a_kd - if self.tuning_mode == 'a': - temporary_kp = self.temp_kp - temporary_ki = self.temp_ki - temporary_kd = self.temp_kd - self.runButtonCallback(mode='a') - - self.temp_kp = self.orig_a_t_kp - self.temp_ki = self.orig_a_t_ki - self.temp_kd = self.orig_a_t_kd - if self.tuning_mode == 'a_t': - temporary_kp = self.temp_kp - temporary_ki = self.temp_ki - temporary_kd = self.temp_kd - self.runButtonCallback(mode='a_t') - - # Reset the current values on the sliders to match the current tuning mode - self.temp_kp = temporary_kp - self.temp_ki = temporary_ki - self.temp_kd = temporary_kd - self.runButtonCallback() - - def saveButtonCallback(self): - if self.disp: print('\nSAVING all parameters') - self.call_originals() - self.clearButtonCallback() - - def undoButtonCallback(self): - self.temp_kp = self.undo_kp - self.temp_ki = self.undo_ki - self.temp_kd = self.undo_kd - self.runButtonCallback() - - def set_previousVals(self): - self._widget.kp_prev_val.setText(f'Previous Value: {self.undo_kp}') - self._widget.ki_prev_val.setText(f'Previous Value: {self.undo_ki}') - self._widget.kd_prev_val.setText(f'Previous Value: {self.undo_kd}') - if self.tuning_mode == 'p': - self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_p_kp}') - self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_p_ki}') - self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_p_kd}') - elif self.tuning_mode == 'c': - self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_c_kp}') - self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_c_ki}') - self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_c_kd}') - elif self.tuning_mode == 'r': - self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_r_kp}') - self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_r_ki}') - self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_r_kd}') - elif self.tuning_mode == 'a': - self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_a_kp}') - self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_a_ki}') - self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_a_kd}') - elif self.tuning_mode == 'a_t': - self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_a_t_kp}') - self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_a_t_ki}') - self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_a_t_kd}') - - def status_sub_callback(self, msg): - if msg.rc_override: - if not self.rc_override: - self.rc_override = True - print('RC OVERRIDE ?!?!?!?') - self.colorChanged.emit(QtGui.QColor(219, 44, 44)) - # self._widget.rcOverrideLabel.setStyleSheet("QLabel {background-color: red; color: white}") - else: - if self.rc_override: - self.rc_override = False - print('not rc override') - self.colorChanged.emit(QtGui.QColor(87, 225, 97)) - # self._widget.rcOverrideLabel.setStyleSheet("QLabel {background-color: green; color: white}") - - @QtCore.pyqtSlot(QtGui.QColor) - def on_color_change(self, color): - self._widget.rcOverrideLabel.setStyleSheet("QLabel {background-color: " + color.name() + "; color: white}") - - def parameter_event_callback(self,msg): - # Look only at the autopilot node - if msg.node == '/autopilot': - for param in msg.changed_parameters: - if param.name == 'c_kp': - self.ckp = param.value.double_value - elif param.name == 'c_ki': - self.cki = param.value.double_value - elif param.name == 'c_kd': - self.ckd = param.value.double_value - elif param.name == 'r_kp': - self.rkp = param.value.double_value - elif param.name == 'r_ki': - self.rki = param.value.double_value - elif param.name == 'r_kd': - self.rkd = param.value.double_value - elif param.name == 'p_kp': - self.pkp = param.value.double_value - elif param.name == 'p_ki': - self.pki = param.value.double_value - elif param.name == 'p_kd': - self.pkd = param.value.double_value - elif param.name == 'a_kp': - self.akp = param.value.double_value - elif param.name == 'a_ki': - self.aki = param.value.double_value - elif param.name == 'a_kd': - self.akd = param.value.double_value - elif param.name == 'a_t_kp': - self.a_t_kp = param.value.double_value - elif param.name == 'a_t_ki': - self.a_t_ki = param.value.double_value - elif param.name == 'a_t_kd': - self.a_t_kd = param.value.double_value - - # Update the text boxes. If updating all of them is too slow, set them one at a time - self._widget.courseReadout.setText(f'c_kp: {self.ckp}\nc_ki: {self.cki}\nc_kd: {self.ckd}') - self._widget.rollReadout.setText(f'r_kp: {self.rkp}\nr_ki: {self.rki}\nr_kd: {self.rkd}') - self._widget.pitchReadout.setText(f'p_kp: {self.pkp}\np_ki: {self.pki}\np_kd: {self.pkd}') - self._widget.altitudeReadout.setText(f'a_kp: {self.akp}\na_ki: {self.aki}\na_kd: {self.akd}') - self._widget.airspeedReadout.setText(f'a_t_kp: {self.a_t_kp}\na_t_ki: {self.a_t_ki}\na_t_kd: {self.a_t_kd}') \ No newline at end of file