From d60c84cde5b7decd94b4364ff85770b711572db9 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 8 Jul 2024 19:40:05 +0200
Subject: [PATCH] Temporary calculations test

Not ran yet on hardware
---
 firmware/src/main.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp
index 06219a2..2d8804b 100644
--- a/firmware/src/main.cpp
+++ b/firmware/src/main.cpp
@@ -25,6 +25,7 @@ Adafruit_DRV2605 haptic;
 SF fusion;
 
 LED led;
+RotationMath rotation_math;
 
 // Variables
 float gx, gy, gz, ax, ay, az, mx, my, mz;
@@ -143,6 +144,7 @@ void loop() {
   throttle = getThrottle();
 
   float* quat = fusion.getQuat();
+  matrix::Eulerf control_angles = rotation_math.getControlAngles(matrix::Quatf(quat));
   // Serial.print(">dt:");
   // Serial.println(deltat);
   // Serial.print(">gx:");
@@ -175,6 +177,14 @@ void loop() {
   Serial.print(">z:");
   Serial.println(quat[3]);
 
+  Serial.print("Roll: ");
+  Serial.print(degrees(control_angles(0)));
+  Serial.print(" Pitch: ");
+  Serial.print(degrees(control_angles(1)));
+  Serial.print(" Yaw: ");
+  Serial.print(degrees(control_angles(2)));
+  Serial.println("");
+
   // animatePowerOn(throttle);
 
   delay(50);