Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 15:17:23 +00:00
Code Releases Activity

Fixed logging errors

Browse Source
This commit is contained in:
bpmcgeeney 2021-10-18 15:22:40 -07:00
parent c3f3c34c5b
commit cebccecf2b
2 changed files with 33 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
include/sim.h
View File

@ -9,7 +9,7 @@ void pidController(struct Vehicle &, struct Vehicle &);
void TVC(struct Vehicle &, struct Vehicle &); void TVC(struct Vehicle &, struct Vehicle &);
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t); void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
void write2CSV(struct outVector &, struct Vehicle &); void write2CSV(struct outVector &, struct Vehicle &, int t);
double derivative(double current, double previous, double step); double derivative(double current, double previous, double step);
double integral(double currentChange, double prevValue, double dt); double integral(double currentChange, double prevValue, double dt);
double limit(double value, double upr, double lwr); double limit(double value, double upr, double lwr);
@ -35,11 +35,13 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
pidController(State, PrevState); pidController(State, PrevState);
TVC(State, PrevState); TVC(State, PrevState);
state2vec(State, PrevState, stateVector, t); state2vec(State, PrevState, stateVector, t);
//std::cout << State.vz << "\n";
t += State.stepSize; t += State.stepSize;
} while ((State.z > 0.0) || (State.thrust > 0.1)); } while ((State.z > 0.0));
std::cout << t << "\n";
write2CSV(stateVector, State); write2CSV(stateVector, State, t);
std::cout << t << "\n";
bool pass = 1; bool pass = 1;
@ -91,9 +93,8 @@ void burnStartTimeCalc(Vehicle &State) {
thrust = -195.78 * i + 675.11; thrust = -195.78 * i + 675.11;
velocity = (((thrust / mass) + g) * dt) + velocity; velocity = (((thrust / mass) + g) * dt) + velocity;
h = velocity * dt + h; h = (((thrust / mass) + g) * dt) + h;
} }
State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
State.burnVelocity = velocity; // terminal velocity State.burnVelocity = velocity; // terminal velocity
@ -340,7 +341,7 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
PrevState = State; PrevState = State;
} }
void write2CSV(outVector &stateVector, Vehicle &State) { void write2CSV(outVector &stateVector, Vehicle &State, int t) {
// Deleting any previous output file // Deleting any previous output file
if (remove("simOut.csv") != 0) if (remove("simOut.csv") != 0)
@ -359,38 +360,38 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
<< std::endl; << std::endl;
// writing to output file // writing to output file
for (int t = 0; t < State.simTime; t += State.stepSize) { for (int i = 0; i < t; i += State.stepSize) {
outfile << t << ", "; outfile << i << ", ";
outfile << stateVector.x[t] << ","; outfile << stateVector.x[i] << ",";
outfile << stateVector.y[t] << ","; outfile << stateVector.y[i] << ",";
outfile << stateVector.z[t] << ","; outfile << stateVector.z[i] << ",";
outfile << stateVector.vx[t] << ","; outfile << stateVector.vx[i] << ",";
outfile << stateVector.vy[t] << ","; outfile << stateVector.vy[i] << ",";
outfile << stateVector.vz[t] << ","; outfile << stateVector.vz[i] << ",";
outfile << stateVector.ax[t] << ","; outfile << stateVector.ax[i] << ",";
outfile << stateVector.ay[t] << ","; outfile << stateVector.ay[i] << ",";
outfile << stateVector.az[t] << ","; outfile << stateVector.az[i] << ",";
outfile << stateVector.yaw[t] * 180 / M_PI << ","; outfile << stateVector.yaw[i] * 180 / M_PI << ",";
outfile << stateVector.pitch[t] * 180 / M_PI << ","; outfile << stateVector.pitch[i] * 180 / M_PI << ",";
outfile << stateVector.roll[t] * 180 / M_PI << ","; outfile << stateVector.roll[i] * 180 / M_PI << ",";
outfile << stateVector.yawdot[t] * 180 / M_PI << ","; outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
outfile << stateVector.pitchdot[t] * 180 / M_PI << ","; outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
outfile << stateVector.rolldot[t] * 180 / M_PI << ","; outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
outfile << stateVector.servo1[t] << ","; outfile << stateVector.servo1[i] << ",";
outfile << stateVector.servo2[t] << ","; outfile << stateVector.servo2[i] << ",";
outfile << stateVector.thrustFiring[t] << ","; outfile << stateVector.thrustFiring[i] << ",";
outfile << stateVector.PIDx[t] << ","; outfile << stateVector.PIDx[i] << ",";
outfile << stateVector.PIDy[t] << ","; outfile << stateVector.PIDy[i] << ",";
outfile << stateVector.thrust[t] << std::endl; outfile << stateVector.thrust[i] << std::endl;
} }
outfile.close(); outfile.close();

5
src/main.cpp
View File

@ -28,9 +28,8 @@ int main() {
State.vz = 0; // [m/s] State.vz = 0; // [m/s]
// Initial YPR // Initial YPR
State.yaw = 10 * M_PI / 180; // [rad]
State.yaw = 75 * M_PI / 180; // [rad] State.pitch = 5 * M_PI / 180; // [rad]
State.pitch = 30 * M_PI / 180; // [rad]
State.roll = 0 * M_PI / 180; // [rad] State.roll = 0 * M_PI / 180; // [rad]
// Initial YPRdot // Initial YPRdot