Merge branch 'main' of gitlab.com:lander-team/lander-cpp

include/sim.h

@@ -1,6 +1,8 @@
 #include "Vehicle.h"
 #include "outVector.h"
 
+#include <iostream>
+
 void burnStartTimeCalc(struct Vehicle &);
 void thrustSelection(struct Vehicle &, int t);
 void pidController(struct Vehicle &, struct Vehicle &);
@@ -38,19 +40,33 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
 
   write2CSV(stateVector, State);
 
-  bool returnValue;
+  bool pass = 1;
 
-  if (abs(State.vz) < 5) {
-    if ((abs(State.yaw) < 5) && (abs(State.pitch) < 5)) {
-      returnValue = 1;
-    } else {
-      returnValue = 0;
-    }
+  double landing_angle =
+      pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
+
+  double landing_velocity =
+      pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);
+
+  if (landing_angle < 5.0) {
+    std::cout << "   Landing Angle < 5°    | PASS | ";
   } else {
-    returnValue = 0;
+    std::cout << "   Landing Angle < 5°    | FAIL | ";
+    pass = pass * 0;
   }
+  std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
+            << std::endl;
 
-  return returnValue;
+  if (landing_velocity < 5.0) {
+    std::cout << "Landing Velocity < 5 m/s | PASS | ";
+  } else {
+    std::cout << "Landing Velocity < 5 m/s | FAIL | ";
+    pass = pass * 0;
+  }
+  std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
+            << State.vz << "]" << std::endl;
+
+  return pass;
 }
 
 void burnStartTimeCalc(Vehicle &State) {
@@ -340,8 +356,6 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
              "thrust, deriv"
           << std::endl;
 
-  std::cout << "Writing to csv...\n";
-
   // writing to output file
   for (int t = 0; t < State.simTime; t += State.stepSize) {
     outfile << t << ", ";
@@ -376,7 +390,7 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
   }
 
   outfile.close();
-  std::cout << "Output File Closed\n";
+  std::cout << "simOut.csv created successfully.\n" << std::endl;
 }
 
 double derivative(double current, double previous, double step) {