diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index d1b5b0f75f..e9cc350398 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -269,4 +269,5 @@ add_custom_command(OUTPUT ${uorb_sources}
 	)
 
 add_library(uorb_msgs ${uorb_sources})
+target_link_libraries(uorb_msgs PRIVATE m)
 add_dependencies(uorb_msgs prebuild_targets uorb_headers)
diff --git a/msg/templates/uorb/msg.cpp.em b/msg/templates/uorb/msg.cpp.em
index faa20dd807..a462f063fd 100644
--- a/msg/templates/uorb/msg.cpp.em
+++ b/msg/templates/uorb/msg.cpp.em
@@ -73,6 +73,8 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so
 #include <uORB/topics/uORBTopics.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/device/Device.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
 
 @# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
 @# This is used for the logger
diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py
index 4c56bdecff..b94048d9ee 100644
--- a/msg/tools/px_generate_uorb_topic_helper.py
+++ b/msg/tools/px_generate_uorb_topic_helper.py
@@ -321,6 +321,13 @@ def print_field(field):
         print("char mag_device_id_buffer[80];")
         print("device::Device::device_id_print_buffer(mag_device_id_buffer, sizeof(mag_device_id_buffer), message.mag_device_id);")
         print("PX4_INFO_RAW(\"\\tmag_device_id: %d (%s) \\n\", message.mag_device_id, mag_device_id_buffer);")
+    elif (field.name == 'q' or 'q_' in field.name) and field.type == 'float32[4]':
+        # float32[4] q/q_d/q_reset/delta_q_reset
+        print("{")
+        print("\t\tmatrix::Eulerf euler{matrix::Quatf{message." + field.name + "}};")
+        print("\t\tPX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + "  (Roll: %.1f deg, Pitch: %.1f deg, Yaw: %.1f deg" ")\\n\", " + field_name + ", (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));\n\t")
+        print("\t}")
+
     elif ("flags" in field.name or "bits" in field.name) and "uint" in field.type:
         # print bits of fixed width unsigned integers (uint8, uint16, uint32) if name contains flags or bits
         print("PX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + " (0b\", " + field_name + ");")