Resolve "Separate native and teensy code"

2025-07-23 14:41:25 +00:00 · 2021-11-03 22:33:10 +00:00
parent ad602ca834
commit 66590a33fe
6 changed files with 273 additions and 220 deletions
--- a/include/native.h
+++ b/include/native.h
@@ -0,0 +1,143 @@
+#include "Vehicle.h"
+
+#include <iostream>
+
+void thrustInfo(struct Vehicle &);
+void processTVC(struct Vehicle &);
+void write2CSV(struct outVector &, struct Vehicle &);
+void printSimResults(struct Vehicle &);
+
+void thrustInfo(Vehicle &State) {
+
+  if (State.burnElapsed != 2000) {
+    // determine where in the thrust curve we're at based on elapsed burn time
+    // as well as current mass
+    State.burnElapsed = (State.time - State.burnStart) / 1000;
+    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
+  }
+
+  else if (abs(State.burnVelocity + State.vz) < 0.001) {
+    // Start burn
+    State.burnStart = State.time;
+    State.burnElapsed = 0;
+  }
+
+  else
+    State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
+
+  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
+    State.thrustFiring = true;
+    State.thrust = 65.165 * State.burnElapsed - 2.3921;
+
+  } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
+    State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
+                   11.609 * pow(State.burnElapsed, 5) +
+                   60.739 * pow(State.burnElapsed, 4) -
+                   162.99 * pow(State.burnElapsed, 3) +
+                   235.6 * pow(State.burnElapsed, 2) -
+                   174.43 * State.burnElapsed + 67.17;
+
+  else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
+    State.thrust = -195.78 * State.burnElapsed - 675.11;
+
+  if (State.burnElapsed > 3.45) {
+    State.thrustFiring = false;
+    State.thrust = 0;
+  }
+}
+
+void processTVC(Vehicle &State) {
+  // Vector math to aqcuire thrust vector components
+  State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
+  State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
+  State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
+             (State.mass * g);
+
+  // Calculate moment created by Fx and Fy
+  State.momentX = State.Fx * State.momentArm;
+  State.momentY = State.Fy * State.momentArm;
+  State.momentZ = 0;
+}
+
+void write2CSV(outVector &stateVector, Vehicle &State) {
+
+  // Deleting any previous output file
+  if (remove("simOut.csv") != 0)
+    perror("No file deletion necessary");
+  else
+    puts("Previous output file successfully deleted");
+
+  // Define and open output file "simOut.csv"
+  std::fstream outfile;
+  outfile.open("simOut.csv", std::ios::app);
+
+  // Output file header. These are the variables that we output - useful for
+  // debugging
+  outfile << "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
+             "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust"
+          << std::endl;
+
+  // writing to output file
+  for (int i = 0; i < State.time; i += State.stepSize) {
+    outfile << i << ", ";
+
+    outfile << stateVector.x[i] << ",";
+    outfile << stateVector.y[i] << ",";
+    outfile << stateVector.z[i] << ",";
+
+    outfile << stateVector.vx[i] << ",";
+    outfile << stateVector.vy[i] << ",";
+    outfile << stateVector.vz[i] << ",";
+
+    outfile << stateVector.ax[i] << ",";
+    outfile << stateVector.ay[i] << ",";
+    outfile << stateVector.az[i] << ",";
+
+    outfile << stateVector.yaw[i] * 180 / M_PI << ",";
+    outfile << stateVector.pitch[i] * 180 / M_PI << ",";
+    outfile << stateVector.roll[i] * 180 / M_PI << ",";
+
+    outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
+    outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
+    outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
+
+    outfile << stateVector.servo1[i] << ",";
+    outfile << stateVector.servo2[i] << ",";
+
+    outfile << stateVector.thrustFiring[i] << ",";
+
+    outfile << stateVector.PIDx[i] << ",";
+    outfile << stateVector.PIDy[i] << ",";
+
+    outfile << stateVector.thrust[i] << std::endl;
+  }
+
+  outfile.close();
+  std::cout << "simOut.csv created successfully.\n" << std::endl;
+}
+
+void printSimResults(Vehicle &State) {
+  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.0 degrees  | PASS | ";
+  } else {
+    std::cout << "Landing Angle     < 5.0 degrees  | FAIL | ";
+  }
+  std::cout << "Final Angles:   [" << State.yaw << ", " << State.pitch << "]"
+            << std::endl;
+
+  if (landing_velocity < 0.5) {
+    std::cout << "Landing Velocity  < 0.5 m/s      | PASS | ";
+  } else {
+    std::cout << "Landing Velocity  < 0.5 m/s      | FAIL | ";
+  }
+  std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
+            << State.vz << "]" << std::endl;
+
+  std::cout << std::endl << "Simulation Complete\n" << std::endl;
+}