From 33d173d1e09dc014cf53742f709f356f5909a31f Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 1 May 2023 11:07:27 +0200
Subject: [PATCH] try to hover

---
 src/px4_connection/src/px4_controller.cpp | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 839853e4..652f95c6 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -60,12 +60,22 @@ private:
     {
         auto msg = px4_msgs::msg::TrajectorySetpoint();
 
-        msg.velocity[0] = 5;
-        msg.velocity[1] = 5;
-        msg.velocity[2] = D_SPEED(1);
+        if (setpoint_count < 30)
+        {
+            msg.velocity[0] = 0;
+            msg.velocity[1] = 0;
+            msg.velocity[2] = D_SPEED(1);
+            msg.yawspeed = 0;
 
-        msg.yaw = -3.14;
-        msg.yawspeed = 0;
+        } else {
+            //try to hover
+            msg.velocity[0] = 0;
+            msg.velocity[1] = 0;
+            msg.velocity[2] = 0;
+            msg.yawspeed = 1;
+
+        }
+            msg.yaw = -3.14;
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
 
         trajectory_setpoint_publisher->publish(msg);