From 4b063ce9b2b6bf052f3143425db6c6f1e51b9687 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 28 Apr 2023 18:36:23 +0200 Subject: [PATCH] try with increasing thrust --- src/px4_connection/src/px4_controller.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 42d8c4e4..e68770b7 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -50,6 +50,7 @@ private: bool has_sent_status = false; bool flying = false; int setpoint_count = 0; + float thrust = 0; /** * @brief Only the attitude is enabled, because that is how the drone will be controlled. @@ -57,9 +58,12 @@ private: */ void send_setpoint() { - - if (setpoint_count < 21) - setpoint_count++; + + setpoint_count++; + + if (setpoint_count % 20 == 0 && thrust <= 1) { + thrust += 0.1; + } if (setpoint_count == 20) { @@ -79,12 +83,12 @@ private: // result quaternion std::array q = {0, 0, 0, 0}; - if (this->get_clock()->now().seconds() - start_time_ < 20) + if (this->get_clock()->now().seconds() - start_time_ < 30) { // move up? msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east - msg.thrust_body[2] = 1; // down, 100% thrust up + msg.thrust_body[2] = thrust; // down, 100% thrust up calculate_quaternion(q, 0, degrees_to_radians(20), 0); }