diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 36cc1788..047eaa5c 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -168,6 +168,7 @@ private:
 
             if (request->control_mode == CONTROL_MODE_VELOCITY)
             {
+                RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", request->x, request->y, request->z);
                 if (trajectory_valid_x)
                     velocity[0] += request->x;
                 if (trajectory_valid_y)
@@ -175,17 +176,18 @@ private:
                 if (trajectory_valid_z)
                     velocity[2] += request->z;
 
-                RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
+                RCLCPP_INFO(this->get_logger(), "Set new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]);
             }
             else if (request->control_mode == CONTROL_MODE_POSITION)
             {
+                RCLCPP_INFO(this->get_logger(), "Got new position setpoint. %f %f %f", request->x, request->y, request->z);
                 if (trajectory_valid_x)
                     position[0] = request->x;
                 if (trajectory_valid_y) 
                     position[1] = request->y;
                 if (trajectory_valid_z)
                     position[2] = request->z;
-                RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]);
+                RCLCPP_INFO(this->get_logger(), "Set new position setpoint: %f %f %f", position[0], position[1], position[2]);
             }
 
             if (trajectory_valid_yaw)