From ac759ace1313425deb56011d989500df63ce5e28 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 1 May 2023 12:38:25 +0200
Subject: [PATCH] change to not check for 30

---
 src/px4_connection/src/px4_controller.cpp | 28 +++++++++++------------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 78b386c1..64e96508 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -59,23 +59,23 @@ private:
     void send_trajectory_setpoint()
     {
         auto msg = px4_msgs::msg::TrajectorySetpoint();
+        msg.velocity[0] = 5;
+        msg.velocity[1] = 5;
+        msg.velocity[2] = -15;
+        msg.yawspeed = 0;
 
-        if (setpoint_count < 30)
-        {
-            msg.velocity[0] = 5;
-            msg.velocity[1] = 5;
-            msg.velocity[2] = 20;
-            msg.yawspeed = 0;
+        // if (setpoint_count < 30)
+        // {
 
-        } else {
-            //try to hover
-            msg.velocity[0] = 0;
-            msg.velocity[1] = 0;
-            msg.velocity[2] = 0;
-            msg.yawspeed = 1;
+        // } else {
+        //     //try to hover
+        //     msg.velocity[0] = 0;
+        //     msg.velocity[1] = 0;
+        //     msg.velocity[2] = 0;
+        //     msg.yawspeed = 1;
 
-        }
-            msg.yaw = -3.14;
+        // }
+        msg.yaw = -3.14;
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
 
         trajectory_setpoint_publisher->publish(msg);