diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index fd145d0aea711a839c5212f3f5c171e1c777b6c2..77da3b66702022776b0f1c9a92fa21c02c191ffb 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -136,6 +136,7 @@ set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/safecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/studentcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/topbanner.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/tuningcontrollertab.h
)
# Flying Agent GUI -- wrap UI file and QOBJECT files
qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
@@ -150,6 +151,7 @@ qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/safecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/studentcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/topbanner.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/tuningcontrollertab.ui
)
qt5_wrap_cpp(SRC_MOC_HDRS_FLYING_AGENT_GUI ${SRC_HDRS_QOBJECT_FLYING_AGENT_GUI})
# Flying Agent GUI -- wrap resource file qrc->rcc
@@ -392,6 +394,7 @@ set(MY_CPP_SOURCES_FLYING_AGENT_GUI # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/safecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/studentcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/topbanner.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/tuningcontrollertab.cpp
)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
index e26851ebaee765dfae8a48dac0b8cba3b5ffdd52..a5fa7abc2ca3f21e0cf99f10274812a995a9bae7 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
@@ -39,8 +39,8 @@
#endif
-#define P_GAIN_MIN_GUI 0.10
-#define P_GAIN_MAX_GUI 1.00
+#define P_GAIN_MIN_GUI 0.05
+#define P_GAIN_MAX_GUI 1.60
#define P_TO_D_GAIN_RATIO_GUI 0.6
#define DECIMAL_PLACES_POSITION_MEASURED 3
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index 9b1127895f60bbb01f91b78ee01434c53f58584c..7d9880e79f6daa068fe0bce04e842d4acec0415c 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -36,6 +36,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->picker_controller_tab_widget , &PickerControllerTab::setMeasuredPose
);
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->tuning_controller_tab_widget , &TuningControllerTab::setMeasuredPose
+ );
+
// CONNECT THE "MEASUREMENTS UNAVAILABLE" SIGNAL TO
@@ -55,6 +60,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->picker_controller_tab_widget , &PickerControllerTab::poseDataUnavailableSlot
);
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->tuning_controller_tab_widget , &TuningControllerTab::poseDataUnavailableSlot
+ );
+
// CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
// WITH THE LIST OF AGENT IDs TO COORDINATE
@@ -76,6 +86,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->picker_controller_tab_widget , &PickerControllerTab::setAgentIDsToCoordinate
);
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->tuning_controller_tab_widget , &TuningControllerTab::setAgentIDsToCoordinate
+ );
+
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
index 7fa20851f61bfe97316799391ac0e98c46ca3768..24ea87a35b4faed13df930563f899bc5b244f2ee 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
@@ -350,7 +350,7 @@ void TuningControllerTab::publishGain(float new_gain)
d_fall_pps::FloatWithHeader msg;
// Fill the header of the message
- fillSetpointMessageHeader( msg );
+ fillFloatMessageHeader( msg );
// Fill in the gain value
msg.data = new_gain;
@@ -463,10 +463,10 @@ void TuningControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
setpointChangedSubscriber.shutdown();
// Set information back to the default
- ui->lineEdit_setpoint_current_x->setText("xx.xx");
- ui->lineEdit_setpoint_current_y->setText("xx.xx");
- ui->lineEdit_setpoint_current_z->setText("xx.xx");
- ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
+ ui->lineEdit_setpoint->setText("xx.xx");
+ //ui->lineEdit_setpoint_current_y->setText("xx.xx");
+ //ui->lineEdit_setpoint_current_z->setText("xx.xx");
+ //ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
}
#endif
@@ -537,7 +537,7 @@ void TuningControllerTab::fillFloatMessageHeader( d_fall_pps::FloatWithHeader &
{
case TYPE_AGENT:
{
- msg.shouldCheckForAgentID = false;
+ msg.shouldCheckForID = false;
break;
}
case TYPE_COORDINATOR:
@@ -545,7 +545,7 @@ void TuningControllerTab::fillFloatMessageHeader( d_fall_pps::FloatWithHeader &
// Lock the mutex
m_agentIDs_toCoordinate_mutex.lock();
// Add the "coordinate all" flag
- msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ msg.shouldCheckForID = !(m_shouldCoordinateAll);
// Add the agent IDs if necessary
if (!m_shouldCoordinateAll)
{
@@ -561,7 +561,7 @@ void TuningControllerTab::fillFloatMessageHeader( d_fall_pps::FloatWithHeader &
default:
{
- msg.shouldCheckForAgentID = true;
+ msg.shouldCheckForID = true;
ROS_ERROR("[TUNING CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
break;
}
diff --git a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
index f6ec1a0285674a3480e3f4787f2d6c725b8b641b..7184651dfec26fa97bb50f0aa618252318564401 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -215,6 +215,8 @@ float m_gain_P_to_D_ratio = 0.6;
+
+
// ******************************************************************************* //
// VARIABLES SPECIFIC TO THE TUNING CONTROL FEATURE
@@ -398,13 +400,13 @@ std::vector<float> PMKF_Kinf_for_angles (2,0.0);
// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
-std::string namespace_to_own_agent_parameter_service;
+std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
+std::string m_namespace_to_coordinator_parameter_service;
// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
-ros::Publisher debugPublisher;
+ros::Publisher m_debugPublisher;
// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
@@ -425,8 +427,11 @@ bool shouldDisplayDebugInfo = false;
// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
// POSITION
-// The ID of this agent, i.e., the ID of this compute
-int my_agentID = 0;
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
diff --git a/pps_ws/src/d_fall_pps/param/TuningController.yaml b/pps_ws/src/d_fall_pps/param/TuningController.yaml
index f427a45df0c74435eac2d62c6cd73a74494f745e..0baaf04eafb2bea1663a35de72c8f1ea7d084fa9 100644
--- a/pps_ws/src/d_fall_pps/param/TuningController.yaml
+++ b/pps_ws/src/d_fall_pps/param/TuningController.yaml
@@ -40,7 +40,7 @@ multiplier_heading_max : 3.0
# Mass of the crazyflie
-mass : 29
+mass : 30
# Frequency of the controller, in hertz
vicon_frequency : 200
@@ -93,7 +93,7 @@ estimator_method : 1
# The LQR Controller parameters for "mode = 3"
-gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.76, 0.00, 0.00, 0.32, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index 90d259e0dd8ff7da4b851f2a9af6e4370252da4b..a92c51dd6aed425414af923c51b6a3a6f71f3c75 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -739,6 +739,7 @@ void setInstantController(int controller) //for right now, temporal use
break;
case TUNING_CONTROLLER:
loadTuningController();
+ break;
case PICKER_CONTROLLER:
loadPickerController();
break;
diff --git a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
index c96fca2dce54e64f7c02ccbd55141b0b851c7aeb..289e3adb8ed838f662b0fc4e629c74217efea0d0 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
@@ -914,15 +914,15 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
- //lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
+ lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
//lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// > ALITUDE CONTROLLER (i.e., z-controller):
lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i] * multiplier_vertical;
}
- lqr_angleRateNested_prev_rollAngle -= ( -1 * m_gain_P[i] * stateErrorBody[1] - m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[4] );
- lqr_angleRateNested_prev_pitchAngle -= ( m_gain_P[i] * stateErrorBody[0] + m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[3] );
+ //lqr_angleRateNested_prev_rollAngle -= ( -1 * m_gain_P * stateErrorBody[1] - m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[4] );
+ lqr_angleRateNested_prev_pitchAngle -= ( m_gain_P * stateErrorBody[0] + m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[3] );
// BODY FRAME Z CONTROLLER
//lqr_angleRateNested_prev_yawAngle = setpoint[3];
@@ -962,6 +962,17 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
}
+
+ float thrustAdjustment_200Hz = 0.0;
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 9; ++i)
+ {
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment_200Hz -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i] * multiplier_vertical;
+ }
+
+
+
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
@@ -973,7 +984,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
- float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
+ float thrustAdjustment = thrustAdjustment_200Hz / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
@@ -1049,7 +1060,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// debugMsg.value_10 = your_variable_name;
// Publish the "debugMsg"
- debugPublisher.publish(debugMsg);
+ m_debugPublisher.publish(debugMsg);
}
@@ -1305,12 +1316,14 @@ void setNewSetpoint(float x, float y, float z, float yaw)
void requestGainChangeCallback(const FloatWithHeader& newGain)
{
// Check whether the message is relevant
- bool isRevelant = checkMessageHeader( m_agentID , newGain.shouldCheckForAgentID , newGain.agentIDs );
+ bool isRevelant = checkMessageHeader( m_agentID , newGain.shouldCheckForID , newGain.agentIDs );
// Continue if the message is relevant
if (isRevelant)
{
m_gain_P = newGain.data;
+
+ ROS_INFO_STREAM("[TUNING CONTROLLER] proportional gain changed to " << m_gain_P );
}
}
@@ -1558,6 +1571,10 @@ int main(int argc, char* argv[]) {
// node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
+ // Get the namespace of this "TuningControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
// AGENT ID AND COORDINATOR ID
// NOTES:
@@ -1729,7 +1746,15 @@ int main(int argc, char* argv[]) {
// And now we can instantiate the subscriber:
ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TuningControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
-
+ // Instantiate the local variable "requestGainChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestGainChange" topic and calls the class function
+ // "requestGainChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the gain is changed
+ // by the user.
+ ros::Subscriber requestGainChangeSubscriber = nodeHandle.subscribe("RequestGainChange", 1, requestGainChangeCallback);