Merge pull request #40 from Propulsive-Landing/formatting-fail

Added sensor formatting to log files

include/Navigation.hpp

@@ -17,6 +17,9 @@ class Navigation
         Eigen::Matrix<double, 12, 1> stateMat;
         std::deque<std::vector<double>> d_theta_queue_reckon;
         double pressureInit;
+        std::tuple<double, double, double> linearAcceleration;
+        std::tuple<double, double, double> angularRate;
+
 
     public:
         double loopTime = 0.005;
@@ -27,5 +30,7 @@ class Navigation
         std::tuple<double,double,double> ComputeAngularRollingAverage(std::vector<double> d_theta_now);
         Eigen::Matrix3d CreateRotationalMatrix(double phi, double theta, double psi);
         double GetHeight();
-        std::tuple<double, double, double> GetBodyAcceleration();
+        std::tuple<double, double, double> GetLinearAcceleration();
+         std::tuple<double, double, double> GetAngularAcceleration();
+
 };

include/Telemetry.hpp

@@ -17,6 +17,8 @@ class Telemetry {
     public:
         std::ofstream Logs;
         std::ofstream HardwareSaved;
+        std::ofstream SensorSaved;
+
 
         void RunTelemetry(Navigation& navigation, Controller& controller, float HardwareSaveDelta, float RFSaveDelta);
         void Log(std::string message);

src/Mode.cpp

@@ -30,7 +30,6 @@ Mode::Mode(Phase eInitialMode) : eCurrentMode(eInitialMode) {}
 Mode::Phase Mode::UpdateCalibration(Navigation& navigation, Controller& controller, double currentTime) {
     static float XTVC = 0.0;
     static float YTVC = 0.0;
-
     RF::Command command = RF::GetInstance().GetCommand();
     if(command == RF::Command::IncrementXTVC){
         XTVC += 0.01;

src/Navigation.cpp

@@ -62,8 +62,8 @@ void Navigation::UpdateNavigation(){
     // Updates stateMat //
 
     // Create 2 tuples to hold the the linear acceleration and angular rate data from the imu 
-    std::tuple<double,double,double> linearAcceleration = imu.GetBodyAcceleration();
-    std::tuple<double,double,double> angularRate = imu.GetBodyAngularRate();
+     linearAcceleration = imu.GetBodyAcceleration();
+     angularRate = imu.GetBodyAngularRate();
 
     // std::cout << "Accel Z:" << std::to_string(std::get<2>(linearAcceleration)) << " gyroX: " << std::to_string(std::get<0>(angularRate)) << "\n";    
     // Convert the linear acceleration tuple to a Vector so we can muliply the Eigen matrix R by another Eigen type which in this case is a vector
@@ -158,8 +158,12 @@ Eigen::Matrix3d Navigation::CreateRotationalMatrix(double phi, double theta, dou
 
 }
 
-std::tuple<double, double, double> Navigation::GetBodyAcceleration()
+std::tuple<double, double, double> Navigation::GetLinearAcceleration()
 {
-    return imu.GetBodyAcceleration();
+    return linearAcceleration;
 }
 
+std::tuple<double, double, double> Navigation::GetAngularAcceleration()
+{
+    return angularRate;
+}

src/Telemetry.cpp

@@ -6,6 +6,7 @@
 #include <stdio.h>
 #include <sys/poll.h> 
 #include <fstream>
+#include <tuple>
 
 #include <nlohmann/json.hpp>
 using json = nlohmann::json;
@@ -22,6 +23,7 @@ void Telemetry::HardwareSaveFrame(Navigation& navigation, Controller& controller
     auto in_time_t = std::chrono::system_clock::to_time_t(time_now);
 
     HardwareSaved << std::put_time(std::localtime(&in_time_t), "%c") << ", " ;
+    SensorSaved << std::put_time(std::localtime(&in_time_t), "%c") << ", " ;
 
     // Navigation state, U, k matrix current index
     // Write data to file
@@ -32,7 +34,18 @@ void Telemetry::HardwareSaveFrame(Navigation& navigation, Controller& controller
     }
     HardwareSaved << std::to_string(controller.GetCurrentIterationIndex());
 
+    std::tuple<double, double, double> linAc = navigation.GetLinearAcceleration();
+    std::tuple<double, double, double> angAc = navigation.GetAngularAcceleration();
+    SensorSaved << std::to_string(std::get<0>(linAc))<< ", ";
+    SensorSaved << std::to_string(std::get<1>(linAc))<< ", ";
+    SensorSaved << std::to_string(std::get<2>(linAc))<< ", ";
+    SensorSaved << std::to_string(std::get<0>(angAc))<< ", ";
+    SensorSaved << std::to_string(std::get<1>(angAc))<< ", ";
+    SensorSaved << std::to_string(std::get<2>(angAc));
+
     HardwareSaved<<"\n" << std::flush;
+    SensorSaved<<"\n" << std::flush;
+
 }
 
 
@@ -96,6 +109,7 @@ void Telemetry::RfSendFrame(Navigation& navigation, Controller& controller)
 
 
 void Telemetry::RunTelemetry(Navigation& navigation, Controller& controller, float HardwareSaveDelta, float RFSaveDelta) {
+
         /* Start calculate time change*/
         static auto last_hardware_time = std::chrono::high_resolution_clock::now();
         auto hardware_change_time = std::chrono::high_resolution_clock::now() - last_hardware_time;
@@ -128,8 +142,12 @@ Telemetry::Telemetry()
 
     Logs.open ("../logs/logs"+str+".txt");
     HardwareSaved.open ("../logs/data"+str+".txt");
+    SensorSaved.open ("../logs/sensors"+str+".txt");
+
 
     HardwareSaved << "Date, x, y, z, vx, vy, vz, phi, theta, psi, p, q, r, K_Matrix_Index \n";
+    SensorSaved << "Date, accelX, accelY, accelZ, gyroX, gryoY, gyroZ \n";
+
 
 
     //TODO Write headers to data file where needed