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 8b0cd5a3ccc35aa3ecefd045b16a1957598b61d2..114d0b7b745ca33fd5d9fee0e333d29941eeafd8 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
@@ -104,6 +104,7 @@ public slots:
private slots:
void newDataForPerformingStepAndPlotting(float x);
+ void analyseStepResponse();
void on_lineEdit_setpoint_new_x_returnPressed();
void on_lineEdit_setpoint_new_y_returnPressed();
@@ -150,6 +151,8 @@ private:
bool m_shouldStoreData_for_plotting = false;
// > 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;
// > Line Series for the data
QLineSeries *m_lineSeries_for_setpoint_x;
QLineSeries *m_lineSeries_for_measured_x;
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 ebea9668c5f9a83a43f963edc69e04a0307996e0..3be53160c55c8dde6c9d897f0703e99e228518c8 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
@@ -70,14 +70,6 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
m_lineSeries_for_setpoint_x = new QLineSeries();
m_lineSeries_for_measured_x = new QLineSeries();
-// // DEBUGGING: SET SOME FAKE DATA FOR TESTING FOR A LINE CHART
-// m_lineSeries_for_setpoint_x->append(0, 6);
-// m_lineSeries_for_setpoint_x->append(2, 4);
-// m_lineSeries_for_setpoint_x->append(3, 8);
-// m_lineSeries_for_setpoint_x->append(7, 4);
-// m_lineSeries_for_setpoint_x->append(10, 5);
-// *m_lineSeries_for_setpoint_x << QPointF(11, 1) << QPointF(13, 3) << QPointF(17, 6) << QPointF(18, 3) << QPointF(20, 2);
-
// Add the line series to the chart
ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_setpoint_x);
ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_measured_x);
@@ -89,7 +81,7 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
// Set the initial axes limits
ui->chartView_for_x->chart()->createDefaultAxes();
ui->chartView_for_x->chart()->axisX()->setMin(-1.0);
- ui->chartView_for_x->chart()->axisX()->setMax(10.0);
+ ui->chartView_for_x->chart()->axisX()->setMax(m_step_response_data_recording_duration);
ui->chartView_for_x->chart()->axisY()->setMin(-0.6);
ui->chartView_for_x->chart()->axisY()->setMax( 0.6);
@@ -334,12 +326,15 @@ void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x)
static float max_value_plotted = 0.01;
static float next_rezoom_at_time = 2.00;
+ // Initialise a bool for whether the step response should be analsysed
+ bool shouldAnalyseStepResponse = false;
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
// Only do something if the flag indicates to do so
if (m_shouldStoreData_for_plotting)
{
- // Lock the mutex
- m_chart_mutex.lock();
-
// Add the data to the line series
float temp_x_setpoint = (ui->lineEdit_setpoint_current_x->text()).toFloat();
m_lineSeries_for_setpoint_x->append(m_time_for_step, (ui->lineEdit_setpoint_current_x->text()).toFloat() );
@@ -366,10 +361,6 @@ void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x)
{
if (m_time_for_step >= -0.002)
{
- // Inform the user about the change
- #ifdef CATKIN_MAKE
- ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] DEBUG 1");
- #endif
// Set the flag so that we do not enter this loop again
m_shouldPerformStep = false;
@@ -397,19 +388,179 @@ void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x)
next_rezoom_at_time += 0.5;
}
- // Stop data collection and analyse the results if the time has passed 10 seconds
- if (m_time_for_step >= 10.002)
+ // Stop data collection and analyse the results if the time has
+ // passed "m_step_response_data_recording_duration" seconds
+ if (m_time_for_step >= (m_step_response_data_recording_duration+0.002))
{
+ // Set the flag to stop data collection
m_shouldStoreData_for_plotting = false;
- // Aslo rezoom the axes
+ // Also rezoom the axes
float diff_of_values_plotted = max_value_plotted - min_value_plotted;
ui->chartView_for_x->chart()->axisY()->setMin( min_value_plotted - 0.1*diff_of_values_plotted );
ui->chartView_for_x->chart()->axisY()->setMax( max_value_plotted + 0.1*diff_of_values_plotted );
+ // Set the flag that the step response should be analysed
+ shouldAnalyseStepResponse = true;
+ // Inform the user that the step is finished
+ #ifdef CATKIN_MAKE
+ ROS_INFO("[CSONE CONTROLLER GUI] Step finished.");
+ #endif
}
+ }
- // Unlock the mutex
- m_chart_mutex.unlock();
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ // Perform analysis of the step response if required
+ if (shouldAnalyseStepResponse)
+ {
+ analyseStepResponse();
+ }
+
+}
+
+void CsoneControllerTab::analyseStepResponse()
+{
+ // Initialise variable for the quantities to be determined
+ float setpoint_start = -999.9;
+ float setpoint_end = -999.9;
+ float rise_time = -999.9;
+ float overshoot_value = -999.9;
+ float settling_time = -999.9;
+ float overshoot_percent = -999.9;
+
+ // RETIEVE THE NECESSARY DATA INTO LOCAL VARIABLES
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Get the length of the line series
+ int count_of_lineSeries_for_setpoint_x = m_lineSeries_for_setpoint_x->count();
+ int count_of_lineSeries_for_measured_x = m_lineSeries_for_measured_x->count();
+
+ // Get all the points
+ QVector<QPointF> vector_of_points_for_setpoint_x = m_lineSeries_for_setpoint_x->pointsVector();
+ QVector<QPointF> vector_of_points_for_measured_x = m_lineSeries_for_measured_x->pointsVector();
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ // PERFORM THE ANALYSIS
+
+ // Only do something if there is actually data to process
+ if ( (count_of_lineSeries_for_setpoint_x<1) && (count_of_lineSeries_for_measured_x<1) )
+ {
+ // Inform the user that the step is finished
+ #ifdef CATKIN_MAKE
+ ROS_ERROR("[CSONE CONTROLLER GUI] Request analyse of the step response, but the line series have a count of zero.");
+ #endif
+ // End this function
+ return;
+ }
+
+ // Get the start and end setpoint
+ if ( count_of_lineSeries_for_setpoint_x>0 )
+ {
+ // Get the start point
+ setpoint_start = vector_of_points_for_setpoint_x.at(0).y();
+ // Get the end point
+ setpoint_end = vector_of_points_for_setpoint_x.at(count_of_lineSeries_for_setpoint_x-1).y();
+ // DEBUGGING: print out the start and end values
+ #ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Setpoint at (start,end) = ( " << setpoint_start << " , " << setpoint_end << " )");
+ #endif
+ }
+
+
+ // Get the rise time, overshoot, and settling time
+ if ( (count_of_lineSeries_for_setpoint_x>0) && (count_of_lineSeries_for_measured_x>0) )
+ {
+ // Only continue if the step was at least 1 centimeter
+ if ( (setpoint_end-setpoint_start)<(-0.01) || (0.01)<(setpoint_end-setpoint_start) )
+ {
+ // Determine if it is a step up of a step down
+ float step_direction = 0.0;
+ if (setpoint_start<setpoint_end)
+ {
+ step_direction = 1.0;
+ }
+ else
+ {
+ step_direction = -1.0;
+ }
+
+ // Compute the threshold for the rise time
+ float rise_time_threshold = setpoint_start + 0.9*(setpoint_end-setpoint_start);
+
+ // Compute the threshold for the settling time
+ float settiling_time_upper_threshold = setpoint_end + step_direction * 0.05 * (setpoint_end-setpoint_start);
+ float settiling_time_lower_threshold = setpoint_end - step_direction * 0.05 * (setpoint_end-setpoint_start);
+
+ // Prepare a few variables needed
+ float this_time = 0.0;
+ float this_val = 0.0;
+ bool riseTimeFound = false;
+ overshoot_value = step_direction*(-100.0);
+
+ // Iterate through the points
+ for (int i = 0; i < vector_of_points_for_measured_x.size(); ++i)
+ {
+ // Get the values for this point
+ this_time = vector_of_points_for_measured_x.at(i).x();
+ this_val = vector_of_points_for_measured_x.at(i).y();
+
+ // Check if the rise time is exceeded
+ if (!riseTimeFound)
+ {
+ // Check if the rise time threshold is exceeded
+ if ( (step_direction*this_val) > (step_direction*rise_time_threshold) )
+ {
+ // Save this as the rise time
+ rise_time = this_time;
+ // Set the flag that it has been found
+ riseTimeFound = true;
+ }
+ }
+
+ // Keep track of the maximum value for the overshoot
+ overshoot_value = step_direction * std::max( step_direction*overshoot_value , step_direction*this_val );
+
+ // Check if outside the settling time threshold
+ if ( (this_val<settiling_time_lower_threshold) || (settiling_time_upper_threshold<this_val) )
+ {
+ // Update the settling time to the time for this iteration
+ settling_time = this_time;
+ }
+ }
+
+ // Convert the settling time to a percentage
+ overshoot_percent = (overshoot_value - setpoint_end) / (setpoint_end-setpoint_start) * 100.0;
+
+ // DEBUGGING: print out the start and end values
+ #ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] (rise time,overshoot value, os percent,settling time) = ( " << rise_time << " , " << overshoot_value << " , " << overshoot_percent << " , " << settling_time << " )");
+ #endif
+ }
}
+
+ // DISPLAY THE RESULTS IN THE FIELDS OF THE GUI
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // For the starting setpoint
+ if (setpoint_start > -999.0)
+ {
+ //ui->...;
+ }
+ else
+ {
+ //ui->...;
+ }
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
}
diff --git a/dfall_ws/src/dfall_pkg/param/CsoneController.yaml b/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
index b7bbda03c3b2d4e8a4183b1289ce48d08cf68b3f..89076bca386c60c1c0d7ec5448e642a8f3302809 100644
--- a/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
@@ -23,6 +23,6 @@ shouldDisplayDebugInfo : false
# The LQR Controller parameters for rate mode
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
-gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
-gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
+gainMatrixRollRate : [ 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00]
+gainMatrixPitchRate : [ 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]