From e6f7e4c160cc91ccf2c2d03400dc12a43281a183 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Sun, 14 Nov 2021 16:41:25 -0700
Subject: [PATCH] Formal build for Avionics Integration Test

---
 include/Vehicle.h | 2 +-
 include/teensy.h  | 6 +++---
 src/main.cpp      | 7 ++++---
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/include/Vehicle.h b/include/Vehicle.h
index 2c37635..ac8b24c 100644
--- a/include/Vehicle.h
+++ b/include/Vehicle.h
@@ -80,7 +80,7 @@ void init_Vehicle(Vehicle &State) {
   State.momentArm = 0.145;       // [m]
 
   // Sim Step Size
-  State.stepSize = 1.0; // [ms]
+  State.stepSize = 20.0; // [ms]
 
   // Other Properties
   State.massPropellant = 0.06;                                  // [kg]
diff --git a/include/teensy.h b/include/teensy.h
index 480711f..e57e6c0 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -240,10 +240,10 @@ void printSimResults(Vehicle &State) {
   Serial.print("Final Angles:   [" + String(State.yaw) + ", " +
                String(State.pitch) + "]\n");
 
-  if (landing_velocity < 0.5) {
-    Serial.print("Landing Velocity  < 0.5 m/s      | PASS | ");
+  if (landing_velocity < 1.0) {
+    Serial.print("Landing Velocity  < 1.0 m/s      | PASS | ");
   } else {
-    Serial.print("Landing Velocity  < 0.5 m/s      | FAIL | ");
+    Serial.print("Landing Velocity  < 1.0 m/s      | FAIL | ");
   }
   Serial.print("Final Velocity: [" + String(State.vx) + ", " +
                String(State.vy) + ", " + String(State.vz) + "]\n");
diff --git a/src/main.cpp b/src/main.cpp
index 47171ae..f55f9e4 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -75,7 +75,7 @@ void setup() {
   testGimbal(servo1, servo2);
   Serial.println("Servos Tested");
 
-  delay(1000);
+  delay(3000);
   Serial.println("Simulated Vehicle Initalized");
 
   // Determine when to burn
@@ -104,17 +104,18 @@ void loop() {
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+    printSimResults(State);
     Serial.println("Run duration:" + String(micros() - initTime) + " us");
 
     closeFile();
-    delay(20000);
+    delay(3000);
 
     Serial.println("SUCCESS");
     Serial.println("Aborting Sim");
     teensyAbort();
   }
 
-  delay(20 - ((micros() - last) * 1000.0));
+  delay(20.0 - ((micros() - last) / 1000.0));
 }
 
 void init_loadCells() {