From 54a77a546c40673bc5667636e7915247edea9c84 Mon Sep 17 00:00:00 2001
From: HLPRED <Harrisonperone@gmail.com>
Date: Sat, 23 Nov 2024 14:33:41 -0500
Subject: [PATCH] Output u in telemetry

---
 include/Controller.hpp | 2 +-
 src/Controller.cpp     | 5 +++++
 src/Telemetry.cpp      | 4 +++-
 3 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/include/Controller.hpp b/include/Controller.hpp
index 4c7a382..dce2cb4 100644
--- a/include/Controller.hpp
+++ b/include/Controller.hpp
@@ -42,5 +42,5 @@ class Controller{
     void ResetKIteration(double current_time);
     int GetCurrentIterationIndex();
     Eigen::Matrix<double, 2, 8>  GetCurrentKMatrix();
-
+    Eigen::Matrix<double, 2, 1>  GetCurrentTVCCommand();
 };
diff --git a/src/Controller.cpp b/src/Controller.cpp
index 2d261ee..6bf27fd 100644
--- a/src/Controller.cpp
+++ b/src/Controller.cpp
@@ -195,3 +195,8 @@ int Controller::GetCurrentIterationIndex()
 {
     return current_iteration_index;
 }
+
+Eigen::Matrix<double, 2, 1> Controller::GetCurrentTVCCommand()
+{
+    return input;
+}
\ No newline at end of file
diff --git a/src/Telemetry.cpp b/src/Telemetry.cpp
index 6231f00..29ccbfc 100644
--- a/src/Telemetry.cpp
+++ b/src/Telemetry.cpp
@@ -32,6 +32,8 @@ void Telemetry::HardwareSaveFrame(Navigation& navigation, Controller& controller
     for(int i = 0; i < 12; ++i){
         HardwareSaved << std::to_string(navigation.GetNavigation()(i))<< ", ";
     }
+    HardwareSaved << std::to_string(controller.GetCurrentTVCCommand()[0]) << ", ";
+    HardwareSaved << std::to_string(controller.GetCurrentTVCCommand()[1]) << ", ";
     HardwareSaved << std::to_string(controller.GetCurrentIterationIndex());
 
     std::tuple<double, double, double> linAc = navigation.GetLinearAcceleration();
@@ -145,7 +147,7 @@ Telemetry::Telemetry()
     SensorSaved.open ("../logs/sensors"+str+".txt");
 
 
-    HardwareSaved << "Date, x, y, z, vx, vy, vz, phi, theta, psi, p, q, r, K_Matrix_Index \n";
+    HardwareSaved << "Date, x, y, z, vx, vy, vz, phi, theta, psi, p, q, r, ux, uy, K_Matrix_Index \n";
     SensorSaved << "Date, accelX, accelY, accelZ, gyroX, gryoY, gyroZ \n";