From 52d19e6b1110b475357786bb3c5b885fcce3de7b Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Wed, 5 Apr 2023 02:37:51 +0300
Subject: [PATCH] Add attitude to log

---
 flix/log.ino | 29 ++++++++++++++++-------------
 1 file changed, 16 insertions(+), 13 deletions(-)

diff --git a/flix/log.ino b/flix/log.ino
index e9e852b..6aa0433 100644
--- a/flix/log.ino
+++ b/flix/log.ino
@@ -1,11 +1,11 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
-#define LOG_RATE 200
-#define LOG_DURATION 10
-#define LOG_PERIOD 1000000 / LOG_RATE
-#define LOG_SIZE LOG_DURATION * LOG_RATE
-#define LOG_COLUMNS 11
+const int LOG_RATE = 100;
+const int LOG_DURATION = 10;
+const int LOG_PERIOD = 1000000 / LOG_RATE;
+const int LOG_SIZE = LOG_DURATION * LOG_RATE;
+const int LOG_COLUMNS = 14;
 
 static float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)
 static int logPointer = 0;
@@ -24,10 +24,13 @@ void logData()
 	logBuffer[logPointer][4] = ratesTarget.x;
 	logBuffer[logPointer][5] = ratesTarget.y;
 	logBuffer[logPointer][6] = ratesTarget.z;
-	logBuffer[logPointer][7] = torqueTarget.x;
-	logBuffer[logPointer][8] = torqueTarget.y;
-	logBuffer[logPointer][9] = torqueTarget.z;
-	logBuffer[logPointer][10] = thrustTarget;
+	logBuffer[logPointer][7] = attitude.toEulerZYX().x;
+	logBuffer[logPointer][8] = attitude.toEulerZYX().y;
+	logBuffer[logPointer][9] = attitude.toEulerZYX().z;
+	logBuffer[logPointer][10] = attitudeTarget.toEulerZYX().x;
+	logBuffer[logPointer][11] = attitudeTarget.toEulerZYX().y;
+	logBuffer[logPointer][12] = attitudeTarget.toEulerZYX().z;
+	logBuffer[logPointer][13] = thrustTarget;
 
 	logPointer++;
 	if (logPointer >= LOG_SIZE) {
@@ -37,12 +40,12 @@ void logData()
 
 void dumpLog()
 {
-	printf("timestamp,rate.x,rate.y,rate.z,target.rate.x,target.rate.y,target.rate.z,torque.x,torque.y,torque.z,thrust\n");
+	Serial.printf("timestamp,rate.x,rate.y,rate.z,target.rate.x,target.rate.y,target.rate.z,"
+		"attitude.x,attitude.y,attitude.z,target.attitude.x,target.attitude.y,target.attitude.z,thrust\n");
 	for (int i = 0; i < LOG_SIZE; i++) {
 		for (int j = 0; j < LOG_COLUMNS - 1; j++) {
-			printf("%f,", logBuffer[i][j]);
+			Serial.printf("%f,", logBuffer[i][j]);
 		}
-		printf("%f\n", logBuffer[i][LOG_COLUMNS - 1]);
+		Serial.printf("%f\n", logBuffer[i][LOG_COLUMNS - 1]);
 	}
-	Serial.println();
 }