From 8acd7f2c73e50370cc9f2951ebe40be1b8952461 Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Fri, 28 Apr 2023 16:50:28 +0200
Subject: [PATCH] add print of quaternion

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

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 33e3b0b6..035a5b60 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -64,6 +64,12 @@ private:
         std::array<float, 4> array = {0, 0, 0, 0};
         calculate_quaternion(array, 0, degrees_to_radians(20), 0);
 
+        std::string elements = "";
+        for (float f : array)
+        {
+            elements += std::to_string(f) + " ";
+        }
+        RCLCPP_INFO(this->get_logger(), "q_d: %s", elements);
         msg.q_d[0] = array[0];
         msg.q_d[1] = array[1];
         msg.q_d[2] = array[2];