diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 047eaa5c..ab31c272 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -382,10 +382,10 @@ private:
             }
             else
             {
-                // if (!new_setpoint)
-                // {
-                //     return;
-                // }
+                if (!new_setpoint)
+                {
+                    return;
+                }
                 if (current_control_mode == CONTROL_MODE_VELOCITY)
                 {
                     RCLCPP_INFO(this->get_logger(), "velocity %f %f %f", velocity[0], velocity[1], velocity[2]);
@@ -396,7 +396,7 @@ private:
                     RCLCPP_INFO(this->get_logger(), "position %f %f %f", position[0], position[1], position[2]);
                     send_position_setpoint();
                 }
-                // new_setpoint = false;
+                new_setpoint = false;
             }
         }
     }