From 5bd2885ad8b91879cd58a7e18e3f81d25c651d08 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Thu, 4 May 2023 14:49:07 +0200
Subject: [PATCH] add print of q_d

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

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 60971e8b..e003c191 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -216,6 +216,8 @@ private:
         msg.q_d[2] = base_q[2] + q.at(2);
         msg.q_d[3] = base_q[3] + q.at(3);
 
+        RCLCPP_INFO(this->get_logger(), "q_d: [%f, %f, %f, %f]", msg.q_d[0], msg.q_d[1], msg.q_d[2], msg.q_d[3]);
+
         msg.yaw_sp_move_rate = 0;
         msg.reset_integral = false;
         msg.fw_control_yaw_wheel = false;
@@ -255,6 +257,13 @@ private:
 
     void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)
     {
+        /*
+        average q:
+        - 0.7313786745071411
+        - 0.005042835604399443
+        - 0.0002370707516092807
+        - 0.6819528937339783
+        */
         if (!armed)
         {
             if (base_q_amount > 2)