Output u in telemetry

Propulsive-Landing · Nov 23, 2024 · 54a77a5 · 54a77a5
1 parent 9859606
commit 54a77a5
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 2 deletions.
diff --git 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
@@ -195,3 +195,8 @@ int Controller::GetCurrentIterationIndex()
 {
     return current_iteration_index;
 }
+
+Eigen::Matrix<double, 2, 1> Controller::GetCurrentTVCCommand()
+{
+    return input;
+}
diff --git 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";