From 8cb032b8d7d272c4b54df27b9435f8d1cd52ec84 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 15 May 2023 10:49:15 +0200
Subject: [PATCH] fix trajectory setpoint control mode checking

---
 src/px4_connection/src/px4_controller.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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;