Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-07-23 14:41:25 +00:00
Code Releases Activity

started debugging

Browse Source
This commit is contained in:
bpmcgeeney
2021-10-11 11:30:51 -07:00
parent 1b13a88c7d
commit 591ddc77e4
6 changed files with 59 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/Vehicle.h
View File

@@ -35,7 +35,6 @@ struct Vehicle {
double yError, yPrevError;
double pError, pPrevError;
double i_yError, i_pError = 0;
int pidTest = 0;
double simTime;
int stepSize;

4
include/outVector.h
View File

@@ -4,7 +4,7 @@
#define OUTVECTOR_H
struct outVector {
int length = 10000; // current sim runs ~5000 steps, x2 just in case
int length = 100000; // current sim runs ~5000 steps, x2 just in case
std::vector<double> x = std::vector<double>(length, 0.0);
std::vector<double> y = std::vector<double>(length, 0.0);
@@ -33,6 +33,8 @@ struct outVector {
std::vector<double> LQRx = std::vector<double>(length, 0.0);
std::vector<double> LQRy = std::vector<double>(length, 0.0);
std::vector<double> thrust = std::vector<double>(length, 0.0);
};
#endif

56
include/sim.h
View File

@@ -14,7 +14,7 @@ double integral(double x2, double x1, double dt);
// Any parameters that are constants should be declared here instead of buried
// in code
double const dt = 0.001;
double const dt = 0.01;
double const g = -9.81;
bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
@@ -29,14 +29,13 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
// Start Sim
do {
thrustSelection(State, t);
// lqrCalc(State);
pidController(State, PrevState);
pidController(State, PrevState); // lqrCalc(State);
TVC(State);
vehicleDynamics(State, PrevState, t);
state2vec(State, stateVector, t);
t++;
} while ((State.z > 0.0) || (State.thrust > 1.0));
t += State.stepSize;
} while ((State.z > 0.0) || (State.thrust > 0.1));
write2CSV(stateVector, State);
@@ -59,7 +58,7 @@ void burnStartTimeCalc(Vehicle &State) {
double velocity = State.vz;
double h = 0;
double a, j, mass, thrust;
double mass, thrust;
// Piecewise functions for F15 thrust curve
for (double i = 0.148; i < 3.450; i = i + dt) {
@@ -107,6 +106,7 @@ void thrustSelection(Vehicle &State, int t) {
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) +
@@ -210,7 +210,6 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
State.Kd * d_yError);
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError);
} else {
State.LQRx = 0;
State.LQRy = 0;
@@ -274,7 +273,7 @@ void TVC(Vehicle &State) {
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
// Idot
if (t < 1) {
if (t < 0.1) {
State.I11dot = 0;
State.I22dot = 0;
State.I33dot = 0;
@@ -285,20 +284,20 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
}
// pdot, qdot, rdot
State.yawddot = (State.momentX - State.I11dot * State.yawdot +
State.I22 * State.pitchdot * State.rolldot -
State.I33 * State.pitchdot * State.rolldot) /
State.yawddot = (State.momentX - State.I11dot * PrevState.yawdot +
State.I22 * PrevState.pitchdot * PrevState.rolldot -
State.I33 * PrevState.pitchdot * PrevState.rolldot) /
State.I11;
State.pitchddot = (State.momentY - State.I22dot * State.pitchdot -
State.I11 * State.rolldot * State.yawdot +
State.I33 * State.rolldot * State.yawdot) /
State.pitchddot = (State.momentY - State.I22dot * PrevState.pitchdot -
State.I11 * PrevState.rolldot * PrevState.yawdot +
State.I33 * PrevState.rolldot * PrevState.yawdot) /
State.I22;
State.rollddot = (State.momentZ - State.I33dot * State.rolldot +
State.I11 * State.pitchdot * State.yawdot -
State.I22 * State.pitchdot * State.yawdot) /
State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
State.I11 * PrevState.pitchdot * PrevState.yawdot -
State.I22 * PrevState.pitchdot * PrevState.yawdot) /
State.I33;
if (t < 1) {
if (t < 0.1) {
State.x = 0;
State.y = 0;
@@ -322,7 +321,7 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
State.az = (State.Fz / State.mass) +
(State.vy * State.yawdot - State.pitchdot * State.vx);
// vx vy vz in Body frame
// vx vy vz in Earth frame
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
State.vz = integral(State.az, PrevState.vz, State.stepSize);
@@ -333,13 +332,14 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
State.z = integral(State.vz, PrevState.z, State.stepSize);
// Euler Angles
State.phidot = State.yawdot + (State.pitchdot * sin(State.yaw) +
State.rolldot * cos(State.yaw)) *
(sin(State.pitch) / cos(State.pitch));
State.phidot =
State.yawdot + (sin(State.pitch) * (State.rolldot * cos(State.yaw) +
State.pitchdot * sin(State.yaw))) /
cos(State.pitch);
State.thetadot =
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.pitch);
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw);
State.psidot =
(State.pitchdot * sin(State.yaw) + State.rolldot * cos(State.yaw)) /
(State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) /
cos(State.pitch);
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
@@ -379,6 +379,7 @@ void state2vec(Vehicle &State, outVector &stateVector, int t) {
stateVector.LQRx[t] = State.LQRx;
stateVector.LQRy[t] = State.LQRy;
stateVector.thrust[t] = State.thrust;
}
void write2CSV(outVector &stateVector, Vehicle &State) {
@@ -396,13 +397,13 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
// 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, LQRx, LQRy"
"pitchdot, rolldot, Servo1, Servo2, thrustFiring, LQRx, LQRy, thrust"
<< std::endl;
std::cout << "Writing to csv...\n";
// writing to output file
for (int t = 0; t < State.simTime; t++) {
for (int t = 0; t < State.simTime; t += State.stepSize) {
outfile << t << ", ";
outfile << stateVector.x[t] << ", ";
@@ -431,7 +432,8 @@ void write2CSV(outVector &stateVector, Vehicle &State) {
outfile << stateVector.thrustFiring[t] << ", ";
outfile << stateVector.LQRx[t] << ", ";
outfile << stateVector.LQRy[t] << std::endl;
outfile << stateVector.LQRy[t] << ", ";
outfile << stateVector.thrust[t] << std::endl;
}
outfile.close();