printing timing info

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