From 03c5b6d16f36827f2868e1615d04a02a94280814 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Fri, 12 Nov 2021 15:05:03 -0700
Subject: [PATCH] printing timing info

---
 include/Vehicle.h |  1 +
 include/teensy.h  | 12 +++++-------
 src/main.cpp      | 13 +++++++++----
 3 files changed, 15 insertions(+), 11 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 62d7076..a6ab093 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) {
@@ -166,14 +166,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 21d20d5..f71edcd 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"
@@ -51,6 +51,8 @@ void setup() {
   delay(1000);
   initFile();
   delay(1000);
+
+  initTime = micros();
 }
 #endif
 
@@ -76,22 +78,23 @@ void loop() {
 #elif defined(TEENSY)
 void loop() {
 
-  last = millis();
+  last = micros();
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
   processTVC(State);
+
+  State.stepDuration = micros() - last;
   write2CSV(State);
 
   // Set "prev" values for next timestep
   PrevState = State;
-
   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);
@@ -100,6 +103,8 @@ void loop() {
     Serial.println("Aborting Sim");
     teensyAbort();
   }
+
+  delay(20 - ((micros() - last) * 1000.0));
 }
 #endif