diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 686d331f..917daf92 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -149,7 +149,7 @@ private:
         const std::shared_ptr<drone_services::srv::SetTrajectory::Request> request,
         const std::shared_ptr<drone_services::srv::SetTrajectory::Response> response)
     {
-        if (request->control_mode != CONTROL_MODE_VELOCITY || request->control_mode != CONTROL_MODE_POSITION)
+        if (!(request->control_mode == CONTROL_MODE_VELOCITY || request->control_mode == CONTROL_MODE_POSITION))
         {
             RCLCPP_INFO(this->get_logger(), "Got invalid trajectory control mode: %d", request->control_mode);
             response->success = false;