From 10191e122bde87fdb7e41b2ce6d23514d9d32ac4 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Fri, 12 May 2023 12:29:31 +0200
Subject: [PATCH] change checking of control_mode

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

diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp
index 13480867..0ed09f55 100644
--- a/src/px4_connection/src/heartbeat.cpp
+++ b/src/px4_connection/src/heartbeat.cpp
@@ -114,7 +114,7 @@ private:
         const std::shared_ptr<drone_services::srv::SetVehicleControl::Request> request,
         const std::shared_ptr<drone_services::srv::SetVehicleControl::Response> response)
     {
-        if (request->control < 0 || request->control > CONTROL_POSITION_POS)
+        if (request->control < 0 || request->control > (1 << CONTROL_POSITION_POS))
         {
             response->status = 1;
         }