From 3e740de48e883031e75fd2b3e8b3005f4ead8274 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 1 May 2023 14:07:09 +0200 Subject: [PATCH] add circling --- src/px4_connection/src/px4_controller.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 6d9c4bf3..7411ca22 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -55,6 +55,7 @@ private: int setpoint_count = 0; float thrust = 0.5; bool ready_to_fly = false; + float cur_yaw = 0; void send_trajectory_setpoint() { @@ -73,11 +74,13 @@ private: RCLCPP_INFO(this->get_logger(), "switching to 0 vel"); has_swithed = true; } - msg.velocity[0] = 1; + + cur_yaw += 1 % 360; + msg.velocity[0] = 0; msg.velocity[1] = 0; msg.velocity[2] = D_SPEED(0); - msg.yawspeed = 1; - msg.yaw = 3.14; + msg.yawspeed = 0.5; + msg.yaw = degrees_to_radians(cur_yaw); } // if (setpoint_count < 30)