diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index e57ee18314ab6ad5716826ea22469a7c0a2d5d22..db7be180f5ffe2fb62ab4e24d390fb14dcbdec64 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -133,7 +133,7 @@ add_message_files(
   CrazyflieEntry.msg
   CrazyflieDB.msg
   #----------------------------------------------------------------------
-  Debugging.msg
+  Debug.msg
 )
 
 ## Generate services in the 'srv' folder
diff --git a/pps_ws/src/d_fall_pps/msg/Debug.msg b/pps_ws/src/d_fall_pps/msg/Debug.msg
new file mode 100644
index 0000000000000000000000000000000000000000..77b25cd113f47258d6309048d56ddecc9b560d20
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/Debug.msg
@@ -0,0 +1,16 @@
+float64 vicon_x
+float64 vicon_y
+float64 vicon_z
+float64 vicon_yaw
+float64 vicon_pitch
+float64 vicon_roll
+float64 value_1
+float64 value_2
+float64 value_3
+float64 value_4
+float64 value_5
+float64 value_6
+float64 value_7
+float64 value_8
+float64 value_9
+float64 value_10
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 6a9603b07870d7092adcdd90275f30c2eff24fad..2d88f411c6b523fe7fb43ea866cbcc5338924394 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -12,6 +12,7 @@
 #include "d_fall_pps/Setpoint.h"
 #include "d_fall_pps/ControlCommand.h"
 #include "d_fall_pps/Controller.h"
+#include "d_fall_pps/Debug.h"
 
 #include <std_msgs/Int32.h>
 
@@ -41,6 +42,8 @@ const float gainMatrixYaw[9] = {0, 0, 0, 0, 0, 0, 0, 0, 2.843099534};
 const float gainMatrixThrust[9] = {0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0};
 
 
+ros::Publisher debugPublisher;
+
 
 // load parameters from corresponding YAML file
 
@@ -113,6 +116,7 @@ void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured)
     -response- is where you have to write your calculated data into.
 */
 bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
+
     //writing the data from -request- to command line
     //might be useful for debugging
     // ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
@@ -123,6 +127,19 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
     // ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
 
+
+    // ************ Fill the debugging message with information to be displayed in rqt_plot ****************
+    Debug debugMsg;
+    debugMsg.vicon_x = request.ownCrazyflie.x;
+    debugMsg.vicon_y = request.ownCrazyflie.y;
+    debugMsg.vicon_z = request.ownCrazyflie.z;
+    debugMsg.vicon_roll = request.ownCrazyflie.roll;
+    debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+    debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+    debugPublisher.publish(debugMsg);
+
+
     // ********* do your calculations here *********
     //Tip: create functions that you call here to keep you code cleaner
     ROS_INFO("custom controller loop");
@@ -232,6 +249,8 @@ int main(int argc, char* argv[]) {
     ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
     ros::Subscriber customYAMLloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/customYAMLloaded", 1, customYAMLloadedCallback);
 
+    ros::Publisher debugPublisher = nodeHandle.advertise<Debug>("Debug", 1);
+
     ROS_INFO("CustomControllerService ready");
     ros::spin();