From 78d5221633cc278ac5a5a6a8f1d1e4fa1680c91a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 3 May 2023 16:40:59 +0200 Subject: [PATCH] add base_q amount --- src/px4_connection/src/px4_controller.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index aa2ac8b6..9165520c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -82,6 +82,9 @@ private: float last_angle = 0; float last_thrust = 0; + float base_q[4] = {0, 0, 0, 0}; + int base_q_amount = 0; + // result quaternion std::array q = {0, 0, 0, 0}; @@ -250,7 +253,14 @@ private: void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) { - + if (!ready_to_fly) + { + base_q_amount++; + base_q[0] = (base_q[0] + msg->q[0])/base_q_amount; + base_q[1] = (base_q[1] + msg->q[1])/base_q_amount; + base_q[2] = (base_q[2] + msg->q[2])/base_q_amount; + base_q[3] = (base_q[3] + msg->q[3])/base_q_amount; + } } /**