mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 07:06:51 +00:00
98 lines
3.3 KiB
C
98 lines
3.3 KiB
C
#include "Vehicle.h"
|
|
|
|
void thrustInfo(struct Vehicle &);
|
|
void processTVC(struct LoadCells &);
|
|
void write2CSV(struct outVector &, struct Vehicle &);
|
|
void printSimResults(struct Vehicle &);
|
|
|
|
void thrustInfo(Vehicle &State) {
|
|
if (State.time == 0) {
|
|
Serial.println("WARNING: thrustInfo not implemented for TEENSY");
|
|
}
|
|
|
|
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, LoadCells &loadCells) {
|
|
if (State.time == 0) {
|
|
Serial.println("WARNING: processTVC not implemented for TEENSY");
|
|
}
|
|
|
|
// Vector math to aqcuire thrust vector components
|
|
// PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
|
|
// PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
|
|
State.Fx = loadCells.lc1Value + loadCells.lc2Value;
|
|
State.Fy = loadCells.lc0Value + loadCells.lc3Value;
|
|
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) {
|
|
|
|
Serial.println("WARNING: write2CSV not implemented for TEENSY");
|
|
}
|
|
|
|
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) {
|
|
Serial.print("Landing Angle < 5.0 degrees | PASS | ");
|
|
} else {
|
|
Serial.print("Landing Angle < 5.0 degrees | FAIL | ");
|
|
}
|
|
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 | ");
|
|
} else {
|
|
Serial.print("Landing Velocity < 0.5 m/s | FAIL | ");
|
|
}
|
|
Serial.print("Final Velocity: [" + String(State.vx) + ", " +
|
|
String(State.vy) + ", " + String(State.vz) + "]\n");
|
|
|
|
Serial.print("\n\nSimulation Complete\n");
|
|
} |