diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
index b9db68b0581266a44ef6644d15d5829746c9939d..03318be61e9ff1ccff46c66daa0128b2addeb8e6 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
@@ -19,13 +19,122 @@
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="6" column="0">
- <widget class="Line" name="line_2">
+ <item row="7" column="0">
+ <spacer name="verticalSpacer_3">
<property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="8" column="0">
+ <widget class="QLabel" name="label_time_series_time_2">
+ <property name="text">
+ <string>Time series for pitch angle in degrees</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QChartView" name="chartView_for_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Maximum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
</property>
</widget>
</item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="red_frame_position_left">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_time_series_time">
+ <property name="text">
+ <string>Time series for x position in meters</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_position_right">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="10" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
<item row="5" column="0">
<layout class="QGridLayout" name="gridLayout_7">
<property name="topMargin">
@@ -300,6 +409,13 @@
</item>
</layout>
</item>
+ <item row="6" column="0">
+ <widget class="Line" name="line_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
<item row="2" column="0">
<spacer name="verticalSpacer_2">
<property name="orientation">
@@ -316,6 +432,9 @@
</property>
</spacer>
</item>
+ <item row="9" column="0">
+ <widget class="QChartView" name="chartView_for_pitch"/>
+ </item>
<item row="1" column="0">
<widget class="Line" name="line">
<property name="orientation">
@@ -323,96 +442,6 @@
</property>
</widget>
</item>
- <item row="3" column="0">
- <layout class="QHBoxLayout" name="horizontalLayout_5">
- <property name="spacing">
- <number>0</number>
- </property>
- <item>
- <widget class="QFrame" name="red_frame_position_left">
- <property name="maximumSize">
- <size>
- <width>10</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="styleSheet">
- <string notr="true">background-color:red;</string>
- </property>
- <property name="frameShape">
- <enum>QFrame::StyledPanel</enum>
- </property>
- <property name="frameShadow">
- <enum>QFrame::Raised</enum>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_time_series_time">
- <property name="text">
- <string>Time series for x position in meters</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QFrame" name="red_frame_position_right">
- <property name="maximumSize">
- <size>
- <width>10</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="styleSheet">
- <string notr="true">background-color:red;</string>
- </property>
- <property name="frameShape">
- <enum>QFrame::StyledPanel</enum>
- </property>
- <property name="frameShadow">
- <enum>QFrame::Raised</enum>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="10" column="0">
- <spacer name="verticalSpacer">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="4" column="0">
- <widget class="QChartView" name="chartView_for_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Maximum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>0</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>16777215</height>
- </size>
- </property>
- </widget>
- </item>
<item row="0" column="0">
<layout class="QGridLayout" name="gridLayout">
<property name="topMargin">
@@ -421,39 +450,7 @@
<property name="rightMargin">
<number>0</number>
</property>
- <item row="0" column="8">
- <widget class="QLabel" name="label_simulation">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Simulation</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_time_delay">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Time Delay</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="10">
+ <item row="1" column="17">
<layout class="QGridLayout" name="gridLayout_8">
<item row="2" column="0">
<widget class="QPushButton" name="reset_integrator_button">
@@ -511,24 +508,388 @@
</item>
</layout>
</item>
- <item row="0" column="9">
- <spacer name="horizontalSpacer_7">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeType">
- <enum>QSizePolicy::Fixed</enum>
- </property>
+ <item row="1" column="9">
+ <layout class="QGridLayout" name="gridLayout_13">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_time_delay_units">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>[milli sec]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QPushButton" name="set_time_delay_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEdit_time_delay">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Source Code Pro</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="19">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
<property name="sizeHint" stdset="0">
<size>
- <width>100</width>
- <height>20</height>
+ <width>13</width>
+ <height>29</height>
</size>
</property>
</spacer>
</item>
- <item row="0" column="5">
- <spacer name="horizontalSpacer_3">
+ <item row="0" column="17">
+ <widget class="QLabel" name="label_integrator">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Integrator</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="18">
+ <spacer name="horizontalSpacer_8">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>13</width>
+ <height>29</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="10">
+ <widget class="QLabel" name="label_pid_controller">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>PD Controller</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="13">
+ <layout class="QGridLayout" name="gridLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_step_size">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Size</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_step_duration">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Duration</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEdit_step_size">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Source Code Pro</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.5</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="lineEdit_step_duration">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Source Code Pro</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>20</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QPushButton" name="perform_step_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Perform</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="log_data_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Log</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="10">
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="1" column="0">
+ <widget class="QPushButton" name="set_pd_controller_parameters_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_5">
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_kp">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>k_p</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="lineEdit_kp">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Source Code Pro</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.5</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="lineEdit_kd">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Source Code Pro</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.2</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_kd">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>k_d</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="14">
+ <spacer name="horizontalSpacer_simulation">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -559,19 +920,112 @@
</property>
</widget>
</item>
+ <item row="0" column="8">
+ <spacer name="horizontalSpacer_pd_controller">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>100</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
<item row="0" column="12">
- <spacer name="horizontalSpacer_2">
+ <spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
<property name="sizeHint" stdset="0">
<size>
- <width>40</width>
+ <width>100</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
+ <item row="0" column="13">
+ <widget class="QLabel" name="label_step_details">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Step Details</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="15">
+ <layout class="QGridLayout" name="gridLayout_10">
+ <item row="2" column="0">
+ <widget class="QPushButton" name="clear_simulation_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Clear</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="simulate_step_response_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Perform</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_simulation_blank">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
<item row="1" column="0">
<layout class="QVBoxLayout" name="verticalLayout">
<item>
@@ -604,7 +1058,7 @@
</property>
<property name="font">
<font>
- <family>Courier</family>
+ <family>Source Code Pro</family>
</font>
</property>
<property name="text">
@@ -657,13 +1111,13 @@
</property>
<property name="maximumSize">
<size>
- <width>240</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
- <family>Courier</family>
+ <family>Source Code Pro</family>
</font>
</property>
<property name="text">
@@ -690,11 +1144,11 @@
</property>
<property name="font">
<font>
- <family>Courier</family>
+ <family>Source Code Pro</family>
</font>
</property>
<property name="text">
- <string>4.0</string>
+ <string>4.00</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -740,93 +1194,12 @@
</item>
</layout>
</item>
- <item row="1" column="3">
- <layout class="QGridLayout" name="gridLayout_6">
- <item row="1" column="0">
- <widget class="QPushButton" name="set_pd_controller_parameters_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>1000</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Set</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <layout class="QGridLayout" name="gridLayout_5">
- <property name="rightMargin">
- <number>0</number>
- </property>
- <item row="0" column="2">
- <widget class="QLabel" name="label_kd">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>k_d</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_kp">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>k_p</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="lineEdit_kd">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>0.2</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
+ <item row="1" column="2">
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <layout class="QGridLayout" name="gridLayout_9">
<item row="1" column="1">
- <widget class="QLineEdit" name="lineEdit_kp">
+ <widget class="QLineEdit" name="lineEdit_alpha_lag">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -835,164 +1208,74 @@
</property>
<property name="maximumSize">
<size>
- <width>240</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
<property name="font">
<font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>0.5</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </item>
- <item row="0" column="6">
- <widget class="QLabel" name="label_step_details">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Step Details</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <layout class="QGridLayout" name="gridLayout_4">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QLabel" name="label_step_size">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Size</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_step_duration">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Duraion</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLineEdit" name="lineEdit_step_size">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>300</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>0.5</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="lineEdit_step_duration">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>300</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>10</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QPushButton" name="perform_step_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>1000</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Perform</string>
- </property>
- </widget>
+ <family>Source Code Pro</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>1.00</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_T_lag">
+ <property name="text">
+ <string>T</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_alpha_lag">
+ <property name="text">
+ <string>alpha</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEdit_T_lag">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Source Code Pro</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.00</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
</item>
- <item row="2" column="1">
- <widget class="QPushButton" name="log_data_button">
+ <item>
+ <widget class="QPushButton" name="set_lag_compensator_parameters_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -1006,30 +1289,14 @@
</size>
</property>
<property name="text">
- <string>Log</string>
+ <string>Set</string>
</property>
</widget>
</item>
</layout>
</item>
- <item row="0" column="1">
- <spacer name="horizontalSpacer_pd_controller">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeType">
- <enum>QSizePolicy::Fixed</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>100</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="0" column="3">
- <widget class="QLabel" name="label_pid_controller">
+ <item row="0" column="15">
+ <widget class="QLabel" name="label_simulation">
<property name="font">
<font>
<weight>75</weight>
@@ -1037,15 +1304,15 @@
</font>
</property>
<property name="text">
- <string>PD Controller</string>
+ <string>Simulation</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="0" column="10">
- <widget class="QLabel" name="label_integrator">
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_lag_compenstor">
<property name="font">
<font>
<weight>75</weight>
@@ -1053,94 +1320,47 @@
</font>
</property>
<property name="text">
- <string>Integrator</string>
+ <string>Lag Compensator</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="0" column="11">
- <spacer name="horizontalSpacer_8">
+ <item row="0" column="16">
+ <spacer name="horizontalSpacer_7">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
<property name="sizeHint" stdset="0">
<size>
- <width>40</width>
+ <width>100</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
- <item row="1" column="2">
- <layout class="QGridLayout" name="gridLayout_13">
- <item row="0" column="0">
- <widget class="QLabel" name="label_time_delay_units">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>[milli sec]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QPushButton" name="set_time_delay_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>1000</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Set</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLineEdit" name="lineEdit_time_delay">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>300</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>0</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- </layout>
+ <item row="0" column="9">
+ <widget class="QLabel" name="label_time_delay">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Time Delay</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
</item>
- <item row="0" column="7">
- <spacer name="horizontalSpacer_simulation">
+ <item row="0" column="1">
+ <spacer name="horizontalSpacer_pd_controller_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -1155,95 +1375,8 @@
</property>
</spacer>
</item>
- <item row="1" column="8">
- <layout class="QGridLayout" name="gridLayout_10">
- <item row="2" column="0">
- <widget class="QPushButton" name="clear_simulation_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>1000</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Clear</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QPushButton" name="simulate_step_response_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>1000</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Perform</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_simulation_blank">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string/>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
</layout>
</item>
- <item row="7" column="0">
- <spacer name="verticalSpacer_3">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeType">
- <enum>QSizePolicy::Fixed</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="9" column="0">
- <widget class="QChartView" name="chartView_for_pitch"/>
- </item>
- <item row="8" column="0">
- <widget class="QLabel" name="label_time_series_time_2">
- <property name="text">
- <string>Time series for pitch angle in degrees</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
</layout>
</widget>
<customwidgets>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
index 9bdef6b6c2c666f61d2e2ddc281977cb7ae477fc..3a372d26f80719c46ad57d16c518f923646e48ac 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
@@ -136,6 +136,8 @@ private slots:
void on_set_lead_compensator_parameters_button_clicked();
+ void on_set_lag_compensator_parameters_button_clicked();
+
void on_set_time_delay_button_clicked();
void on_toggle_integrator_button_clicked();
@@ -144,6 +146,8 @@ private slots:
void on_lineEdit_k_returnPressed();
void on_lineEdit_T_returnPressed();
void on_lineEdit_alpha_returnPressed();
+ void on_lineEdit_T_lag_returnPressed();
+ void on_lineEdit_alpha_lag_returnPressed();
void on_lineEdit_kp_returnPressed();
void on_lineEdit_kd_returnPressed();
void on_lineEdit_time_delay_returnPressed();
@@ -153,6 +157,8 @@ private slots:
void on_lineEdit_k_editingFinished();
void on_lineEdit_T_editingFinished();
void on_lineEdit_alpha_editingFinished();
+ void on_lineEdit_T_lag_editingFinished();
+ void on_lineEdit_alpha_lag_editingFinished();
void on_lineEdit_step_size_editingFinished();
void on_lineEdit_step_duration_editingFinished();
@@ -189,6 +195,9 @@ private:
// > Mutex for serialising acess to the controller variables
QMutex m_controller_parameter_mutex;
+ // > Mutex for serialising acess to the lag controller variables
+ QMutex m_lag_controller_parameter_mutex;
+
// FOR PLOTTING DATA ON THE CHART
// > Mutex for serialising acess to any charting variable
QMutex m_chart_mutex;
@@ -201,7 +210,7 @@ private:
// > Time (as a float) for the horizontal axis
float m_time_for_step = 0.0f;
// > The duration for which to record the step
- float m_step_response_data_recording_duration = 10.0f;
+ float m_step_response_data_recording_duration = 20.0f;
// > Line Series for the x position data
QLineSeries *m_lineSeries_for_setpoint_x;
QLineSeries *m_lineSeries_for_measured_x;
@@ -226,6 +235,15 @@ private:
// The state of lead compensator controller
float m_lead_compensator_state = 0.0;
+ // The state space matrices of lag compensator controller
+ float m_lag_compensator_A=0.0;
+ float m_lag_compensator_B=0.0;
+ float m_lag_compensator_C=0.0;
+ float m_lag_compensator_D=1.0;
+
+ // The state of lead compensator controller
+ float m_lag_compensator_state = 0.0;
+
// Line Series for plotting the simulation results
QLineSeries *m_lineSeries_for_sim_setpoint_x;
QLineSeries *m_lineSeries_for_sim_measured_x;
@@ -239,7 +257,10 @@ private:
ros::Publisher requestSetpointChangePublisher;
// > For requesting the controller paramters to be changed
- ros::Publisher requestControllerParamterChangePublisher;
+ ros::Publisher requestControllerParameterChangePublisher;
+
+ // > For requesting the lag controller paramters to be changed
+ ros::Publisher requestLagControllerParameterChangePublisher;
// > For requesting a change in the integrator state
ros::Publisher requestIntegratorStateChangePublisher;
@@ -282,7 +303,9 @@ private:
void publishSetpoint(float x, float y, float z, float yaw_degrees);
- void publishControllerParamters(float k, float T, float alpha);
+ void publishControllerParameters(float k, float T, float alpha);
+
+ void publishLagControllerParameters(float T, float alpha);
void publishRequestForIntegratorStateChange(int flag_to_publish);
@@ -290,6 +313,8 @@ private:
void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
+ void convertLagIntoDiscreteTimeParameters(float T, float alpha);
+
};
#endif // CSONECONTROLLERTAB_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
index 37b301055cf06de4b73da0178a9e67f72a8f7d3e..74500f5ac1a12efb76f815dffdc2fb8400a84bec 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
@@ -56,6 +56,13 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
ui->lineEdit_kd->hide();
ui->set_pd_controller_parameters_button->hide();
+ // ------------------------------------------------------------- //
+ // HIDE THE INTEGRATOR FIELDS
+ ui->label_integrator->hide();
+ ui->label_integrator_state->hide();
+ ui->toggle_integrator_button->hide();
+ ui->reset_integrator_button->hide();
+
// ------------------------------------------------------------- //
// HIDE THE SIMULATION BUTTONS
@@ -193,10 +200,13 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
// CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("CsoneControllerService/RequestSetpointChange", 1);
- // CREATE THE REQUEST CONTROLLER PARAMTER CHANGE PUBLISHER
- requestControllerParamterChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("CsoneControllerService/RequestControllerParametersChange", 1);
+ // CREATE THE REQUEST CONTROLLER PARAMETER CHANGE PUBLISHER
+ requestControllerParameterChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("CsoneControllerService/RequestControllerParametersChange", 1);
+
+ // CREATE THE REQUEST LAG CONTROLLER PARAMETER CHANGE PUBLISHER
+ requestLagControllerParameterChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("CsoneControllerService/RequestLagControllerParametersChange", 1);
- // CREATE THE REQUEST CONTROLLER PARAMTER CHANGE PUBLISHER
+ // CREATE THE REQUEST TIME DELAY CHANGE PUBLISHER
requestTimeDelayChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("CsoneControllerService/RequestTimeDelayChange", 1);
// CREATE THE REQUEST INTEGRATOR STATE CHANGE PUBLISHER
@@ -323,7 +333,7 @@ void CsoneControllerTab::on_perform_step_button_clicked()
ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
// Update the duration from the field
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
// Set the minimum of the x-axis to agree with the duration
ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
@@ -377,7 +387,7 @@ void CsoneControllerTab::on_log_data_button_clicked()
ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
// Update the duration from the field
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
// Set the minimum of the x-axis to agree with the duration
ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
@@ -498,7 +508,7 @@ void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
k = validate_and_get_value_from_lineEdit(ui->lineEdit_k,-100.0,100.0,4,0.016);
// > For T
- T = validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,1,4.0);
+ T = validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,2,4.0);
// > For alpha
alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.1,1.0,2,0.1);
@@ -507,10 +517,29 @@ void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
m_controller_parameter_mutex.unlock();
// Call the function to publish the controller parameters
- publishControllerParamters(k,T,alpha);
+ publishControllerParameters(k,T,alpha);
}
+void CsoneControllerTab::on_set_lag_compensator_parameters_button_clicked()
+{
+ // Initialise local variable for each of (x,y,z,yaw)
+ float T = 0.0f, alpha = 1.0f;
+
+ // Lock the mutex
+ m_lag_controller_parameter_mutex.lock();
+
+ // > For T
+ T = validate_and_get_value_from_lineEdit(ui->lineEdit_T_lag,0.0,100.0,2,0.0);
+
+ // > For alpha
+ alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha_lag,1.0,100.0,2,1.0);
+ // Unlock the mutex
+ m_lag_controller_parameter_mutex.unlock();
+
+ // Call the function to publish the controller parameters
+ publishLagControllerParameters(T,alpha);
+}
void CsoneControllerTab::on_set_time_delay_button_clicked()
{
@@ -555,6 +584,16 @@ void CsoneControllerTab::on_lineEdit_alpha_returnPressed()
ui->set_lead_compensator_parameters_button->animateClick();
}
+void CsoneControllerTab::on_lineEdit_T_lag_returnPressed()
+{
+ ui->set_lag_compensator_parameters_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_alpha_lag_returnPressed()
+{
+ ui->set_lag_compensator_parameters_button->animateClick();
+}
+
void CsoneControllerTab::on_lineEdit_kp_returnPressed()
{
ui->set_pd_controller_parameters_button->animateClick();
@@ -593,7 +632,7 @@ void CsoneControllerTab::on_lineEdit_k_editingFinished()
void CsoneControllerTab::on_lineEdit_T_editingFinished()
{
m_controller_parameter_mutex.lock();
- validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,1,4.0);
+ validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,2,4.0);
m_controller_parameter_mutex.unlock();
}
@@ -604,7 +643,19 @@ void CsoneControllerTab::on_lineEdit_alpha_editingFinished()
m_controller_parameter_mutex.unlock();
}
+void CsoneControllerTab::on_lineEdit_T_lag_editingFinished()
+{
+ m_lag_controller_parameter_mutex.lock();
+ validate_and_get_value_from_lineEdit(ui->lineEdit_T_lag,0.0,100.0,2,0.0);
+ m_lag_controller_parameter_mutex.unlock();
+}
+void CsoneControllerTab::on_lineEdit_alpha_lag_editingFinished()
+{
+ m_lag_controller_parameter_mutex.lock();
+ validate_and_get_value_from_lineEdit(ui->lineEdit_alpha_lag,1.0,100.0,2,1.0);
+ m_lag_controller_parameter_mutex.unlock();
+}
void CsoneControllerTab::on_lineEdit_step_size_editingFinished()
{
@@ -616,7 +667,7 @@ void CsoneControllerTab::on_lineEdit_step_size_editingFinished()
void CsoneControllerTab::on_lineEdit_step_duration_editingFinished()
{
m_chart_mutex.lock();
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
m_chart_mutex.unlock();
}
@@ -1107,7 +1158,7 @@ void CsoneControllerTab::publishSetpoint(float x, float y, float z, float yaw_de
-void CsoneControllerTab::publishControllerParamters(float k, float T, float alpha)
+void CsoneControllerTab::publishControllerParameters(float k, float T, float alpha)
{
#ifdef CATKIN_MAKE
// Initialise the message as a local variable
@@ -1122,7 +1173,7 @@ void CsoneControllerTab::publishControllerParamters(float k, float T, float alph
msg.z = alpha;
// Publish the setpoint
- this->requestControllerParamterChangePublisher.publish(msg);
+ this->requestControllerParameterChangePublisher.publish(msg);
// Inform the user about the change
ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for new controller parameters (k,T,alpha) = (" << k << ", "<< T << ", "<< alpha << ")");
@@ -1136,7 +1187,32 @@ void CsoneControllerTab::publishControllerParamters(float k, float T, float alph
}
+void CsoneControllerTab::publishLagControllerParameters(float T, float alpha)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+ // Fill in the controller values
+ msg.y = T;
+ msg.z = alpha;
+
+ // Publish the setpoint
+ this->requestLagControllerParameterChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for new lag controller parameters (T,alpha) = (" << T << ", "<< alpha << ")");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for new lag controller parameters (T,alpha): [" << T << ", "<< alpha << "]";
+#endif
+
+ // Call the function to convert this to a discrete-time state-space controller
+ convertLagIntoDiscreteTimeParameters(T, alpha);
+}
void CsoneControllerTab::publishRequestForIntegratorStateChange(int flag_to_publish)
@@ -1237,6 +1313,45 @@ void CsoneControllerTab::convertIntoDiscreteTimeParameters(float k, float T, flo
}
+// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void CsoneControllerTab::convertLagIntoDiscreteTimeParameters(float T, float alpha)
+{
+ float control_frequency = 200.0;
+
+ if (alpha > 100){alpha = 100;} else if (alpha<1){alpha = 1;}
+
+ if (T > 100){T = 100;} else if (T<0){T = 0;}
+
+ // Lock the mutex
+ m_simulation_mutex.lock();
+
+ // Compute the A,B,C,D matrices
+ if (T > 0){
+ m_lag_compensator_A = pow(2.71828,(-1.0/(control_frequency*alpha*T)));
+ m_lag_compensator_B = -alpha*T*(m_lag_compensator_A-1.0);
+ m_lag_compensator_C = (alpha - 1.0)/(alpha*T);
+ }
+ else{
+ m_lag_compensator_A = 0;
+ m_lag_compensator_B = 0;
+ m_lag_compensator_C = 0;
+ }
+ m_lag_compensator_D = 1;
+
+ // Reset the state of the lead compensator to zero
+ m_lag_compensator_state = 0.0;
+
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] Lag parameters changed to T=" << T << ", alpha=" << alpha << "\n";
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] Lag matrices changed to A=" << m_lag_compensator_A << ", B=" << m_lag_compensator_B << ", C=" << m_lag_compensator_C << ", D=" << m_lag_compensator_D;
+#endif
+
+ // Unlock the mutex
+ m_simulation_mutex.unlock();
+
+}
void CsoneControllerTab::simulate_step_response()
@@ -1253,6 +1368,13 @@ void CsoneControllerTab::simulate_step_response()
float controller_state = 0.0;
+ float lag_A = m_lag_compensator_A;
+ float lag_B = m_lag_compensator_B;
+ float lag_C = m_lag_compensator_C;
+ float lag_D = m_lag_compensator_D;
+
+ float lag_controller_state = 0.0;
+
// Unlock the mutex
m_simulation_mutex.unlock();
@@ -1265,7 +1387,7 @@ void CsoneControllerTab::simulate_step_response()
float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
- m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,40.0,0,20.0);
float duration = m_step_response_data_recording_duration;
// Unlock the mutex
@@ -1404,11 +1526,14 @@ void CsoneControllerTab::simulate_step_response()
float x_error = x_reference_end - x_for_this_time_step;
// Compute the input to apply
- // > FIRST: compute the new pitch reference
- float pitch_ref = C*controller_state + D*x_error;
- // > SECOND: evaluate the state update equation
+ // > FIRST: compute the lead controller intermediate output
+ float lead_u = C*controller_state + D*x_error;
+ // > SECOND: compute the new pitch reference
+ float pitch_ref = lag_C*lag_controller_state + lag_D*lead_u;
+ // > THIRD: evaluate the state update equation
controller_state = A*controller_state + B*x_error;
- // > THIRD: perform the inner controller
+ lag_controller_state = lag_A*lag_controller_state + lag_B*lead_u;
+ // > FOURTH: perform the inner controller
float pitch_rate = 5.0*(pitch_ref-current_pitch);
// A =
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index cdaa0bfd6d967b3acf51c11da88505a0fc629a5f..1734530a2d3ce02dada4c5fa9206b1941c810e9d 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -185,12 +185,21 @@ float m_cf_weight_in_newtons = 0.0;
// The state space matrices of lead compensator controller
float m_A=0.967216;
float m_B=0.00491758;
-float m_C=-12;
-float m_D=2;
+float m_C=-12.0;
+float m_D=2.0;
// The state of lead compensator controller
float m_controller_state = 0.0;
+// The state space matrices of lag compensator controller
+float m_lag_A=0.0;
+float m_lag_B=0.0;
+float m_lag_C=0.0;
+float m_lag_D=1.0;
+
+// The state of lag compensator controller
+float m_lag_controller_state = 0.0;
+
// The inertial x measurement during a time window of 300 measurements
std::vector<float> m_x_inertial_buffer (300,0);
@@ -280,9 +289,15 @@ void setNewTimeDelay(int newTimeDelay);
// REQUEST CONTROLLER PARAMETERS CALLBACK
void requestControllerParametersChangeCallback(const SetpointWithHeader& newSetpoint);
+// REQUEST LAG CONTROLLER PARAMETERS CALLBACK
+void requestLagControllerParametersChangeCallback(const SetpointWithHeader& newSetpoint);
+
// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
+// CHANGE LAG CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void convertLagIntoDiscreteTimeParameters(float T, float alpha);
+
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 64ffed038626413fb95d1cb51ffa06115c027875..2c625ef48f29a82c2d2de868528ec2f316f7a38c 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -306,19 +306,23 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// For the x-controller:
- // > Execute the discrete-time state space equivalent of the lead
+ // > Execute the discrete-time state space equivalent of the lead-lag
// compensator specified.
- // > NOTE: the new pitch reference, i.e., the output to the state
- // space, MUST be calucated before the state update equation
+ // > NOTE: the new pitch reference, i.e., the output to the controller,
+ // MUST be calucated before the state update equation
// is evaulated, in equations:
- // FIRST: y(t) = C x(t) + D u(t)
- // SECOND: x(t+1) = A x(t) + B u(t)
+ // FIRST: u_ld(t) = C_ld x_ld(t) + D_ld e(t)
+ // y(t) = C_lg x_lg(t) + D_lg u_ld(t)
+ // SECOND: x_ld(t+1) = A_ld x_ld(t) + B_ld e(t)
+ // x_lg(t+1) = A_lg x_lg(t) + B_lg u_ld(t)
// > FIRST: compute the new pitch reference
- float pitch_ref = m_C*m_controller_state - m_D*stateErrorBody[0];
+ float lead_u = m_C*m_controller_state - m_D*stateErrorBody[0];
+ float pitch_ref = m_lag_C*m_lag_controller_state + m_lag_D*lead_u;
// > SECOND: evaluate the state update equation
m_controller_state = m_A*m_controller_state - m_B*stateErrorBody[0];
+ m_lag_controller_state = m_lag_A*m_lag_controller_state + m_lag_B*lead_u;
// > THIRD: perform the inner controller
pitchRate_forResponse = 5.0*(pitch_ref-request.ownCrazyflie.pitch);
@@ -793,6 +797,21 @@ void requestControllerParametersChangeCallback(const SetpointWithHeader& newSetp
}
}
+// REQUEST LAG CONTROLLER PARAMETERS CHANGE CALLBACK
+// Using y z from SetpointWithHeader for T alpha controller parameters
+void requestLagControllerParametersChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ convertLagIntoDiscreteTimeParameters(newSetpoint.y, newSetpoint.z);
+ m_lag_controller_state = 0.0;
+ }
+}
+
// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
{
@@ -811,7 +830,30 @@ void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
}
+// CHANGE LAG CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void convertLagIntoDiscreteTimeParameters(float T, float alpha)
+{
+ if (alpha > 100){alpha = 100;} else if (alpha<1){alpha = 1;}
+
+ if (T > 100){T = 100;} else if (T<0){T = 0;}
+ if (T > 0){
+ m_lag_A = pow(2.71828,(-1.0/(yaml_control_frequency*alpha*T)));
+ m_lag_B = -alpha*T*(m_lag_A-1.0);
+ m_lag_C = (alpha-1.0)/(alpha*T);
+ }
+ else{
+ m_lag_A = 0;
+ m_lag_B = 0;
+ m_lag_C = 0;
+ }
+ m_lag_D = 1;
+
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Lag parameters changed to T="<<T<<", alpha="<<alpha);
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Lag matrices changed to A="<<m_lag_A<<", B="<<m_lag_B<<", C="<<m_lag_C<<", D="<<m_lag_D);
+
+
+}
// ----------------------------------------------------------------------------------
@@ -1215,6 +1257,7 @@ int main(int argc, char* argv[]) {
// messages from the "flying agent GUI" when the setpoint is changed
// by the user.
ros::Subscriber requestControllerParametersChangeSubscriber = nodeHandle.subscribe("RequestControllerParametersChange", 1, requestControllerParametersChangeCallback);
+ ros::Subscriber requestLagControllerParametersChangeSubscriber = nodeHandle.subscribe("RequestLagControllerParametersChange", 1, requestLagControllerParametersChangeCallback);
// Instantiate the class variable "m_setpointChangedPublisher" to
// be a "ros::Publisher". This variable advertises under the name