From 6b33ded940196c7fd9fd00a73e2c5d49a5c2de72 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 1 May 2023 10:56:43 +0200
Subject: [PATCH] add compensation for earth gravity pull

---
 src/px4_connection/src/px4_controller.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 1ed4e6ff..4632cb9e 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -19,6 +19,8 @@ https://discuss.px4.io/t/cannot-arm-drone-with-companion-computer-arming-denied-
 #include <px4_msgs/msg/vehicle_control_mode.hpp>
 // #include <px4_msgs/msg/offboard_control_mode.hpp>
 
+#define D_SPEED(x) x + 9.81
+
 using namespace std::chrono_literals;
 
 class PX4Controller : public rclcpp::Node
@@ -60,7 +62,7 @@ private:
 
         msg.velocity[0] = 2;
         msg.velocity[1] = 0;
-        msg.velocity[2] = 1;
+        msg.velocity[2] = D_SPEED(9.81);
 
         msg.yaw = -3.14;
         msg.yawspeed = 0;