From 4594c87fe6ec73a50c6b245b8bf3946334c08377 Mon Sep 17 00:00:00 2001
From: Brendan McGeeney <bpmcgeeney@gmail.com>
Date: Fri, 12 Nov 2021 22:23:30 +0000
Subject: [PATCH] Print Timing Info

---
 include/Vehicle.h |  1 +
 include/teensy.h  | 12 +++++-------
 src/main.cpp      | 19 ++++++++++++++++---
 3 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/include/Vehicle.h b/include/Vehicle.h
index 921c953..a5de21e 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -41,6 +41,7 @@ struct Vehicle {
 
   double simTime;
   double stepSize;
+  double stepDuration;
 
   double time = 0.0;
 };
diff --git a/include/teensy.h b/include/teensy.h
index c069d64..b2056b9 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -75,7 +75,7 @@ void initFile() {
   // File Header
   dataFile.println(
       "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
-      "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust");
+      "Servo1,Servo2,thrustFiring,thrust,simResponse");
 }
 
 void thrustInfo(Vehicle &State) {
@@ -168,14 +168,12 @@ void write2CSV(Vehicle &State) {
 
   dataFile.print(String(State.thrustFiring, 5));
   dataFile.print(",");
-
-  dataFile.print(String(State.PIDx, 5));
-  dataFile.print(",");
-  dataFile.print(String(State.PIDy, 5));
-  dataFile.print(",");
-
   dataFile.print(String(State.thrust, 5));
+  dataFile.print(",");
+
+  dataFile.print(String(State.stepDuration, 5));
   dataFile.print("\n");
+
 }
 
 void printSimResults(Vehicle &State) {
diff --git a/src/main.cpp b/src/main.cpp
index 17ff880..4382951 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -12,7 +12,7 @@
 #include <Arduino.h>
 
 int BUILTIN_LED = 13;
-unsigned long last;
+unsigned long last, initTime;
 #endif
 
 #include "Vehicle.h"
@@ -75,6 +75,8 @@ void setup() {
   delay(1000);
   initFile();
   delay(1000);
+
+  initTime = micros();
 }
 #endif
 
@@ -100,19 +102,28 @@ void loop() {
 #elif defined(TEENSY)
 void loop() {
 
-  last = millis();
+  last = micros();
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
+  
   processTVC(State, loadCells);
+
+  State.stepDuration = micros() - last;
+  write2CSV(State);
+
+  // Set "prev" values for next timestep
+  PrevState = State;
+  
   // state2vec(State, PrevState, stateVector);
 
+
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     printSimResults(State);
-    Serial.println("Run duration:" + String(millis() - last) + " ms");
+    Serial.println("Run duration:" + String(micros() - initTime) + " us");
 
     closeFile();
     delay(20000);
@@ -121,6 +132,8 @@ void loop() {
     Serial.println("Aborting Sim");
     teensyAbort();
   }
+
+  delay(20 - ((micros() - last) * 1000.0));
 }
 #endif