mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
update include structure
This commit is contained in:
parent
24bf3f2102
commit
5233a9ef2e
10
.vscode/tasks.json
vendored
10
.vscode/tasks.json
vendored
@ -4,10 +4,10 @@
|
||||
{
|
||||
"type": "cppbuild",
|
||||
"label": "Debug",
|
||||
"command": "C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe",
|
||||
"command": "g++",
|
||||
"args": [
|
||||
"-g",
|
||||
"${workspaceFolder}\\src\\*.cpp",
|
||||
"${workspaceFolder}\\src\\main.cpp",
|
||||
"-o",
|
||||
"${workspaceFolder}\\build\\debug\\${fileBasenameNoExtension}.exe",
|
||||
"-I",
|
||||
@ -23,14 +23,13 @@
|
||||
"kind": "build",
|
||||
"isDefault": true
|
||||
},
|
||||
"detail": "compiler: \"C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe\""
|
||||
},
|
||||
{
|
||||
"type": "cppbuild",
|
||||
"label": "Release",
|
||||
"command": "C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe",
|
||||
"command": "g++",
|
||||
"args": [
|
||||
"${workspaceFolder}\\src\\*.cpp",
|
||||
"${workspaceFolder}\\src\\main.cpp",
|
||||
"-o",
|
||||
"${workspaceFolder}\\build\\release\\${fileBasenameNoExtension}.exe",
|
||||
"-I",
|
||||
@ -46,7 +45,6 @@
|
||||
"kind": "build",
|
||||
"isDefault": true
|
||||
},
|
||||
"detail": "compiler: \"C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe\""
|
||||
}
|
||||
]
|
||||
}
|
||||
429
include/sim.h
429
include/sim.h
@ -7,4 +7,431 @@ void TVC(struct sVars &, double g);
|
||||
void vehicleDynamics(struct sVars &, int t);
|
||||
void write2CSV(struct sVars &, std::fstream &outfile, int t);
|
||||
double derivative(double x2, double x1, double dt);
|
||||
double integral(double x2, double x1, double dt);
|
||||
double integral(double x2, double x1, double dt);
|
||||
|
||||
void sim(struct sVars &Vars) {
|
||||
double g = -9.81;
|
||||
|
||||
// defining a few random values here cause I'm lazy
|
||||
Vars.burnElapsed = 2000;
|
||||
Vars.m = Vars.m0;
|
||||
Vars.thrust_prev = 0;
|
||||
|
||||
burnStartTimeCalc(Vars, g);
|
||||
|
||||
// Deleting any previous output file
|
||||
if (remove("simOut.csv") != 0)
|
||||
perror("Error deleting file");
|
||||
else
|
||||
puts("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, yawddot, pitchddot, rollddot, I11, I22, I33, "
|
||||
"I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
|
||||
"LQRx, LQRy"
|
||||
<< std::endl;
|
||||
|
||||
// Start Sim
|
||||
for (int t = 0; t < Vars.simTime; t++) {
|
||||
thrustSelection(Vars, t);
|
||||
lqrCalc(Vars);
|
||||
TVC(Vars, g);
|
||||
vehicleDynamics(Vars, t);
|
||||
write2CSV(Vars, outfile, t);
|
||||
}
|
||||
|
||||
outfile.close();
|
||||
}
|
||||
|
||||
void burnStartTimeCalc(struct sVars &Vars, double g) {
|
||||
double v = Vars.vz;
|
||||
double h = 0;
|
||||
double dt = 0.001;
|
||||
double a, j, m, thrust;
|
||||
|
||||
for (double i = 0.148; i < 3.450; i = i + dt) {
|
||||
m = Vars.m0 - i * Vars.mdot;
|
||||
|
||||
if ((i > 0.147) & (i < 0.420))
|
||||
thrust = 65.165 * i - 2.3921;
|
||||
|
||||
else if ((i > 0.419) & (i < 3.383))
|
||||
thrust = 0.8932 * pow(i, 6) - 11.609 * pow(i, 5) + 60.739 * pow(i, 4) -
|
||||
162.99 * pow(i, 3) + 235.6 * pow(i, 2) - 174.43 * i + 67.17;
|
||||
|
||||
else if ((i > 3.382) & (i < 3.46))
|
||||
thrust = -195.78 * i + 675.11;
|
||||
|
||||
v = (((thrust / m) + g) * dt) + v;
|
||||
h = v * dt + h;
|
||||
}
|
||||
|
||||
Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height
|
||||
Vars.vb = v; // terminal velocity
|
||||
|
||||
double burnStartTime = Vars.vb / -g;
|
||||
Vars.simTime = (Vars.tb + burnStartTime) * 1000;
|
||||
}
|
||||
|
||||
void thrustSelection(struct sVars &Vars, int t) {
|
||||
double tol = 0.001; // 0.001 seems to be a nice tolerance
|
||||
|
||||
// Check to see if current velocity is close to the F15's total velocity
|
||||
bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) &
|
||||
(Vars.vb > (1 - tol) * Vars.vz * -1);
|
||||
|
||||
if (Vars.burnElapsed != 2000) {
|
||||
// determine where in the thrust curve we're at based on elapsed burn time
|
||||
// as well as current mass
|
||||
Vars.burnElapsed = (t - Vars.burnStart) / 1000;
|
||||
Vars.m = Vars.m0 - (Vars.mdot * Vars.burnElapsed);
|
||||
}
|
||||
|
||||
else if (b_burnStart) {
|
||||
// Start burn
|
||||
Vars.burnStart = t;
|
||||
Vars.burnElapsed = 0;
|
||||
}
|
||||
|
||||
else
|
||||
Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn
|
||||
|
||||
if ((Vars.burnElapsed > 0.147) & (Vars.burnElapsed < 0.420))
|
||||
Vars.thrust = 65.165 * Vars.burnElapsed - 2.3921;
|
||||
|
||||
else if ((Vars.burnElapsed > 0.419) & (Vars.burnElapsed < 3.383))
|
||||
Vars.thrust =
|
||||
0.8932 * pow(Vars.burnElapsed, 6) - 11.609 * pow(Vars.burnElapsed, 5) +
|
||||
60.739 * pow(Vars.burnElapsed, 4) - 162.99 * pow(Vars.burnElapsed, 3) +
|
||||
235.6 * pow(Vars.burnElapsed, 2) - 174.43 * Vars.burnElapsed + 67.17;
|
||||
|
||||
else if ((Vars.burnElapsed > 3.382) & (Vars.burnElapsed < 3.46))
|
||||
Vars.thrust = -195.78 * Vars.burnElapsed + 675.11;
|
||||
}
|
||||
|
||||
void lqrCalc(struct sVars &Vars) {
|
||||
|
||||
Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
|
||||
pow(Vars.vehicleRadius, 2) / 4);
|
||||
Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
|
||||
pow(Vars.vehicleRadius, 2) / 4);
|
||||
Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2);
|
||||
|
||||
/*
|
||||
|
||||
double n = sqrt(398600 / pow(6678, 3));
|
||||
double k1 = (Vars.I22 - Vars.I33) / Vars.I11;
|
||||
double k2 = (Vars.I11 - Vars.I33) / Vars.I22;
|
||||
double k3 = (Vars.I22 - Vars.I11) / Vars.I33;
|
||||
|
||||
double R11 = pow(10, 2);
|
||||
double R22 = pow(10, 2);
|
||||
double R33 = pow(10, 2);
|
||||
|
||||
double F11 = -2 * pow(n, 2) * 4 * k1;
|
||||
double F22 = -2 * pow(n, 2) * 3 * k2;
|
||||
double F33 = -2 * pow(n, 2) * k3;
|
||||
|
||||
double G31 = n * (1 - k1);
|
||||
double G13 = n * (k3 - 1);
|
||||
|
||||
double d = 0.5;
|
||||
/*
|
||||
The following calculations are based on output of LQR.m to avoid working
|
||||
with matrices in C++ Please see .m file for details on calculation.
|
||||
Algorithm taken from LQR wikipedia page:
|
||||
https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
|
||||
*/
|
||||
/*
|
||||
double gain = 0.25;
|
||||
|
||||
double K11 =
|
||||
-1 * gain *
|
||||
(Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) +
|
||||
R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) /
|
||||
(R11 * d * Vars.I11 *
|
||||
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
|
||||
double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22);
|
||||
double K33 =
|
||||
-1 * gain *
|
||||
(Vars.I33 *
|
||||
(Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) +
|
||||
R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) /
|
||||
(R33 * d * pow(abs(Vars.I33), 2) *
|
||||
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
|
||||
double K34 =
|
||||
gain * (R11 * pow(abs(Vars.I11), 2) * G13) /
|
||||
(R33 * Vars.I33 *
|
||||
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
|
||||
double K16 =
|
||||
gain * (R33 * pow(abs(Vars.I33), 2) * G31) /
|
||||
(R11 * Vars.I11 *
|
||||
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
|
||||
|
||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
|
||||
Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm;
|
||||
Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm;
|
||||
|
||||
*/
|
||||
|
||||
// Paste in Values from gainCalc.m
|
||||
double K11 = 39.54316;
|
||||
double K12 = 0.00000;
|
||||
double K13 = -0.00000;
|
||||
double K14 = 39.55769;
|
||||
double K15 = 0.00000;
|
||||
double K16 = 0.00000;
|
||||
double K21 = 0.00000;
|
||||
double K22 = 39.54316;
|
||||
double K23 = 0.00000;
|
||||
double K24 = 0.00000;
|
||||
double K25 = 39.55769;
|
||||
double K26 = 0.00000;
|
||||
double K31 = 0.00000;
|
||||
double K32 = 0.00000;
|
||||
double K33 = 39.54316;
|
||||
double K34 = 0.00000;
|
||||
double K35 = 0.00000;
|
||||
double K36 = 39.54394;
|
||||
|
||||
double gain =
|
||||
0.25 *
|
||||
pow(10, -4); // changing exponenet drastically changes results of LQR
|
||||
|
||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
|
||||
Vars.LQRx =
|
||||
gain *
|
||||
((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 +
|
||||
K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) /
|
||||
-Vars.momentArm;
|
||||
Vars.LQRy =
|
||||
gain *
|
||||
((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 +
|
||||
K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) /
|
||||
-Vars.momentArm;
|
||||
|
||||
// LQR Force limiter X
|
||||
if (Vars.LQRx > Vars.thrust)
|
||||
Vars.LQRx = Vars.thrust;
|
||||
else if (Vars.LQRx < -1 * Vars.thrust)
|
||||
Vars.LQRx = -1 * Vars.thrust;
|
||||
|
||||
// LQR Force limiter Y
|
||||
if (Vars.LQRy > Vars.thrust)
|
||||
Vars.LQRy = Vars.thrust;
|
||||
else if (Vars.LQRy < -1 * Vars.thrust)
|
||||
Vars.LQRy = -1 * Vars.thrust;
|
||||
}
|
||||
|
||||
void TVC(struct sVars &Vars, double g) {
|
||||
if (Vars.thrust < 1) {
|
||||
// Define forces and moments for t = 0
|
||||
Vars.Fx = 0;
|
||||
Vars.Fy = 0;
|
||||
Vars.Fz = g * Vars.m0;
|
||||
|
||||
Vars.momentX = 0;
|
||||
Vars.momentY = 0;
|
||||
Vars.momentZ = 0;
|
||||
}
|
||||
|
||||
else {
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
Vars.xServoDegs = (180 / 3.1416) * asin(Vars.LQRx / Vars.thrust);
|
||||
|
||||
// Servo position limiter
|
||||
if (Vars.xServoDegs > Vars.maxServo)
|
||||
Vars.xServoDegs = Vars.maxServo;
|
||||
else if (Vars.xServoDegs < -1 * Vars.maxServo)
|
||||
Vars.xServoDegs = -1 * Vars.maxServo;
|
||||
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
Vars.yServoDegs = (180 / 3.1416) * asin(Vars.LQRy / Vars.thrust);
|
||||
|
||||
// Servo position limiter
|
||||
if (Vars.yServoDegs > Vars.maxServo)
|
||||
Vars.yServoDegs = Vars.maxServo;
|
||||
else if (Vars.yServoDegs < -1 * Vars.maxServo)
|
||||
Vars.yServoDegs = -1 * Vars.maxServo;
|
||||
|
||||
// Vector math to aqcuire thrust vector components
|
||||
Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (3.1416 / 180));
|
||||
Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (3.1416 / 180));
|
||||
Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) +
|
||||
(Vars.m * g);
|
||||
|
||||
// Calculate moment created by Fx and Fy
|
||||
Vars.momentX = Vars.Fx * Vars.momentArm;
|
||||
Vars.momentY = Vars.Fy * Vars.momentArm;
|
||||
Vars.momentZ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void vehicleDynamics(struct sVars &Vars, int t) {
|
||||
// Idot
|
||||
if (t < 1) {
|
||||
Vars.I11dot = 0;
|
||||
Vars.I22dot = 0;
|
||||
Vars.I33dot = 0;
|
||||
} else {
|
||||
Vars.I11dot = derivative(Vars.I11, Vars.I11prev, Vars.stepSize);
|
||||
Vars.I22dot = derivative(Vars.I22, Vars.I22prev, Vars.stepSize);
|
||||
Vars.I33dot = derivative(Vars.I33, Vars.I33prev, Vars.stepSize);
|
||||
}
|
||||
|
||||
// pdot, qdot, rdot
|
||||
Vars.yawddot = (Vars.momentX - Vars.I11dot * Vars.yawdot +
|
||||
Vars.I22 * Vars.pitchdot * Vars.rolldot -
|
||||
Vars.I33 * Vars.pitchdot * Vars.rolldot) /
|
||||
Vars.I11;
|
||||
Vars.pitchddot = (Vars.momentY - Vars.I22dot * Vars.pitchdot -
|
||||
Vars.I11 * Vars.rolldot * Vars.yawdot +
|
||||
Vars.I33 * Vars.rolldot * Vars.yawdot) /
|
||||
Vars.I22;
|
||||
Vars.rollddot = (Vars.momentZ - Vars.I33dot * Vars.rolldot +
|
||||
Vars.I11 * Vars.pitchdot * Vars.yawdot -
|
||||
Vars.I22 * Vars.pitchdot * Vars.yawdot) /
|
||||
Vars.I33;
|
||||
|
||||
if (t < 1) {
|
||||
Vars.x = 0;
|
||||
Vars.y = 0;
|
||||
|
||||
Vars.ax = 0;
|
||||
Vars.ay = 0;
|
||||
Vars.az = Vars.Fz / Vars.m0;
|
||||
}
|
||||
|
||||
else {
|
||||
// p, q, r
|
||||
Vars.yawdot = integral(Vars.yawddot, Vars.yawdotPrev, Vars.stepSize);
|
||||
Vars.pitchdot = integral(Vars.pitchddot, Vars.pitchdotPrev, Vars.stepSize);
|
||||
Vars.rolldot = integral(Vars.rollddot, Vars.rolldotPrev, Vars.stepSize);
|
||||
|
||||
// ax ay az
|
||||
Vars.ax =
|
||||
(Vars.Fx / Vars.m) + (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy);
|
||||
Vars.ay =
|
||||
(Vars.Fy / Vars.m) + (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot);
|
||||
Vars.az =
|
||||
(Vars.Fz / Vars.m) + (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx);
|
||||
|
||||
// vx vy vz in Body frame
|
||||
Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize);
|
||||
Vars.vy = integral(Vars.ay, Vars.vyPrev, Vars.stepSize);
|
||||
Vars.vz = integral(Vars.az, Vars.vzPrev, Vars.stepSize);
|
||||
|
||||
// Xe
|
||||
Vars.x = integral(Vars.vx, Vars.xPrev, Vars.stepSize);
|
||||
Vars.y = integral(Vars.vy, Vars.yPrev, Vars.stepSize);
|
||||
Vars.z = integral(Vars.vz, Vars.zPrev, Vars.stepSize);
|
||||
|
||||
// Euler Angles
|
||||
Vars.phidot = Vars.yawdot + (Vars.pitchdot * sin(Vars.yaw) +
|
||||
Vars.rolldot * cos(Vars.yaw)) *
|
||||
(sin(Vars.pitch) / cos(Vars.pitch));
|
||||
Vars.thetadot =
|
||||
Vars.pitchdot * cos(Vars.yaw) - Vars.rolldot * sin(Vars.pitch);
|
||||
Vars.psidot =
|
||||
(Vars.pitchdot * sin(Vars.yaw) + Vars.rolldot * cos(Vars.yaw)) /
|
||||
cos(Vars.pitch);
|
||||
|
||||
Vars.yaw = integral(Vars.phidot, Vars.yawPrev, Vars.stepSize);
|
||||
Vars.pitch = integral(Vars.thetadot, Vars.pitchPrev, Vars.stepSize);
|
||||
Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize);
|
||||
}
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
Vars.I11prev = Vars.I11;
|
||||
Vars.I22prev = Vars.I22;
|
||||
Vars.I33prev = Vars.I33;
|
||||
|
||||
Vars.yawPrev = Vars.yaw;
|
||||
Vars.pitchPrev = Vars.pitch;
|
||||
Vars.rollPrev = Vars.roll;
|
||||
|
||||
Vars.yawdotPrev = Vars.yawdot;
|
||||
Vars.pitchdotPrev = Vars.pitchdot;
|
||||
Vars.rolldotPrev = Vars.rolldot;
|
||||
|
||||
Vars.yawddotPrev = Vars.yawddot;
|
||||
Vars.pitchddotPrev = Vars.pitchddot;
|
||||
Vars.rollddotPrev = Vars.rollddot;
|
||||
|
||||
Vars.axPrev = Vars.ax;
|
||||
Vars.ayPrev = Vars.ay;
|
||||
Vars.azPrev = Vars.az;
|
||||
|
||||
Vars.vxPrev = Vars.vx;
|
||||
Vars.vyPrev = Vars.vy;
|
||||
Vars.vzPrev = Vars.vz;
|
||||
|
||||
Vars.xPrev = Vars.x;
|
||||
Vars.yPrev = Vars.y;
|
||||
Vars.zPrev = Vars.z;
|
||||
}
|
||||
|
||||
void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
|
||||
// writing to output file
|
||||
outfile << t << ", ";
|
||||
|
||||
outfile << Vars.x << ", ";
|
||||
outfile << Vars.y << ", ";
|
||||
outfile << Vars.z << ", ";
|
||||
|
||||
outfile << Vars.vx << ", ";
|
||||
outfile << Vars.vy << ", ";
|
||||
outfile << Vars.vz << ", ";
|
||||
|
||||
outfile << Vars.ax << ", ";
|
||||
outfile << Vars.ay << ", ";
|
||||
outfile << Vars.az << ", ";
|
||||
|
||||
outfile << Vars.yaw * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.pitch * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.roll * 180 / 3.1416 << ", ";
|
||||
|
||||
outfile << Vars.yawdot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.pitchdot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.rolldot * 180 / 3.1416 << ", ";
|
||||
|
||||
outfile << Vars.yawddot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.pitchddot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.rollddot * 180 / 3.1416 << ", ";
|
||||
|
||||
outfile << Vars.I11 << ", ";
|
||||
outfile << Vars.I22 << ", ";
|
||||
outfile << Vars.I33 << ", ";
|
||||
|
||||
outfile << Vars.I11dot << ", ";
|
||||
outfile << Vars.I22dot << ", ";
|
||||
outfile << Vars.I33dot << ", ";
|
||||
|
||||
outfile << Vars.xServoDegs << ", ";
|
||||
outfile << Vars.yServoDegs << ", ";
|
||||
|
||||
outfile << Vars.m << ", ";
|
||||
outfile << Vars.thrust << ", ";
|
||||
outfile << Vars.burnElapsed << ", ";
|
||||
outfile << Vars.Fz << ", ";
|
||||
|
||||
outfile << Vars.LQRx << ", ";
|
||||
outfile << Vars.LQRy << std::endl;
|
||||
}
|
||||
|
||||
double derivative(double x2, double x1, double dt) {
|
||||
double dxdt = (x2 - x1) / (dt / 1000);
|
||||
return dxdt;
|
||||
}
|
||||
|
||||
double integral(double x, double y, double dt) {
|
||||
double integ = (x * dt / 1000) + y;
|
||||
return integ;
|
||||
}
|
||||
@ -1,6 +1,10 @@
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "sVars.h"
|
||||
#include "sim.h"
|
||||
#include <iostream>
|
||||
|
||||
void sim(struct sVars &);
|
||||
|
||||
|
||||
432
src/sim.cpp
432
src/sim.cpp
@ -1,432 +0,0 @@
|
||||
#include "sim.h"
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <stdio.h>
|
||||
|
||||
void sim(struct sVars &Vars) {
|
||||
double g = -9.81;
|
||||
|
||||
// defining a few random values here cause I'm lazy
|
||||
Vars.burnElapsed = 2000;
|
||||
Vars.m = Vars.m0;
|
||||
Vars.thrust_prev = 0;
|
||||
|
||||
burnStartTimeCalc(Vars, g);
|
||||
|
||||
// Deleting any previous output file
|
||||
if (remove("simOut.csv") != 0)
|
||||
perror("Error deleting file");
|
||||
else
|
||||
puts("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, yawddot, pitchddot, rollddot, I11, I22, I33, "
|
||||
"I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
|
||||
"LQRx, LQRy"
|
||||
<< std::endl;
|
||||
|
||||
// Start Sim
|
||||
for (int t = 0; t < Vars.simTime; t++) {
|
||||
thrustSelection(Vars, t);
|
||||
lqrCalc(Vars);
|
||||
TVC(Vars, g);
|
||||
vehicleDynamics(Vars, t);
|
||||
write2CSV(Vars, outfile, t);
|
||||
}
|
||||
|
||||
outfile.close();
|
||||
}
|
||||
|
||||
void burnStartTimeCalc(struct sVars &Vars, double g) {
|
||||
double v = Vars.vz;
|
||||
double h = 0;
|
||||
double dt = 0.001;
|
||||
double a, j, m, thrust;
|
||||
|
||||
for (double i = 0.148; i < 3.450; i = i + dt) {
|
||||
m = Vars.m0 - i * Vars.mdot;
|
||||
|
||||
if ((i > 0.147) & (i < 0.420))
|
||||
thrust = 65.165 * i - 2.3921;
|
||||
|
||||
else if ((i > 0.419) & (i < 3.383))
|
||||
thrust = 0.8932 * pow(i, 6) - 11.609 * pow(i, 5) + 60.739 * pow(i, 4) -
|
||||
162.99 * pow(i, 3) + 235.6 * pow(i, 2) - 174.43 * i + 67.17;
|
||||
|
||||
else if ((i > 3.382) & (i < 3.46))
|
||||
thrust = -195.78 * i + 675.11;
|
||||
|
||||
v = (((thrust / m) + g) * dt) + v;
|
||||
h = v * dt + h;
|
||||
}
|
||||
|
||||
Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height
|
||||
Vars.vb = v; // terminal velocity
|
||||
|
||||
double burnStartTime = Vars.vb / -g;
|
||||
Vars.simTime = (Vars.tb + burnStartTime) * 1000;
|
||||
}
|
||||
|
||||
void thrustSelection(struct sVars &Vars, int t) {
|
||||
double tol = 0.001; // 0.001 seems to be a nice tolerance
|
||||
|
||||
// Check to see if current velocity is close to the F15's total velocity
|
||||
bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) &
|
||||
(Vars.vb > (1 - tol) * Vars.vz * -1);
|
||||
|
||||
if (Vars.burnElapsed != 2000) {
|
||||
// determine where in the thrust curve we're at based on elapsed burn time
|
||||
// as well as current mass
|
||||
Vars.burnElapsed = (t - Vars.burnStart) / 1000;
|
||||
Vars.m = Vars.m0 - (Vars.mdot * Vars.burnElapsed);
|
||||
}
|
||||
|
||||
else if (b_burnStart) {
|
||||
// Start burn
|
||||
Vars.burnStart = t;
|
||||
Vars.burnElapsed = 0;
|
||||
}
|
||||
|
||||
else
|
||||
Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn
|
||||
|
||||
if ((Vars.burnElapsed > 0.147) & (Vars.burnElapsed < 0.420))
|
||||
Vars.thrust = 65.165 * Vars.burnElapsed - 2.3921;
|
||||
|
||||
else if ((Vars.burnElapsed > 0.419) & (Vars.burnElapsed < 3.383))
|
||||
Vars.thrust =
|
||||
0.8932 * pow(Vars.burnElapsed, 6) - 11.609 * pow(Vars.burnElapsed, 5) +
|
||||
60.739 * pow(Vars.burnElapsed, 4) - 162.99 * pow(Vars.burnElapsed, 3) +
|
||||
235.6 * pow(Vars.burnElapsed, 2) - 174.43 * Vars.burnElapsed + 67.17;
|
||||
|
||||
else if ((Vars.burnElapsed > 3.382) & (Vars.burnElapsed < 3.46))
|
||||
Vars.thrust = -195.78 * Vars.burnElapsed + 675.11;
|
||||
}
|
||||
|
||||
void lqrCalc(struct sVars &Vars) {
|
||||
|
||||
Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
|
||||
pow(Vars.vehicleRadius, 2) / 4);
|
||||
Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
|
||||
pow(Vars.vehicleRadius, 2) / 4);
|
||||
Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2);
|
||||
|
||||
/*
|
||||
|
||||
double n = sqrt(398600 / pow(6678, 3));
|
||||
double k1 = (Vars.I22 - Vars.I33) / Vars.I11;
|
||||
double k2 = (Vars.I11 - Vars.I33) / Vars.I22;
|
||||
double k3 = (Vars.I22 - Vars.I11) / Vars.I33;
|
||||
|
||||
double R11 = pow(10, 2);
|
||||
double R22 = pow(10, 2);
|
||||
double R33 = pow(10, 2);
|
||||
|
||||
double F11 = -2 * pow(n, 2) * 4 * k1;
|
||||
double F22 = -2 * pow(n, 2) * 3 * k2;
|
||||
double F33 = -2 * pow(n, 2) * k3;
|
||||
|
||||
double G31 = n * (1 - k1);
|
||||
double G13 = n * (k3 - 1);
|
||||
|
||||
double d = 0.5;
|
||||
/*
|
||||
The following calculations are based on output of LQR.m to avoid working
|
||||
with matrices in C++ Please see .m file for details on calculation.
|
||||
Algorithm taken from LQR wikipedia page:
|
||||
https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
|
||||
*/
|
||||
/*
|
||||
double gain = 0.25;
|
||||
|
||||
double K11 =
|
||||
-1 * gain *
|
||||
(Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) +
|
||||
R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) /
|
||||
(R11 * d * Vars.I11 *
|
||||
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
|
||||
double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22);
|
||||
double K33 =
|
||||
-1 * gain *
|
||||
(Vars.I33 *
|
||||
(Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) +
|
||||
R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) /
|
||||
(R33 * d * pow(abs(Vars.I33), 2) *
|
||||
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
|
||||
double K34 =
|
||||
gain * (R11 * pow(abs(Vars.I11), 2) * G13) /
|
||||
(R33 * Vars.I33 *
|
||||
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
|
||||
double K16 =
|
||||
gain * (R33 * pow(abs(Vars.I33), 2) * G31) /
|
||||
(R11 * Vars.I11 *
|
||||
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
|
||||
|
||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
|
||||
Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm;
|
||||
Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm;
|
||||
|
||||
*/
|
||||
|
||||
// Paste in Values from gainCalc.m
|
||||
double K11 = 39.54316;
|
||||
double K12 = 0.00000;
|
||||
double K13 = -0.00000;
|
||||
double K14 = 39.55769;
|
||||
double K15 = 0.00000;
|
||||
double K16 = 0.00000;
|
||||
double K21 = 0.00000;
|
||||
double K22 = 39.54316;
|
||||
double K23 = 0.00000;
|
||||
double K24 = 0.00000;
|
||||
double K25 = 39.55769;
|
||||
double K26 = 0.00000;
|
||||
double K31 = 0.00000;
|
||||
double K32 = 0.00000;
|
||||
double K33 = 39.54316;
|
||||
double K34 = 0.00000;
|
||||
double K35 = 0.00000;
|
||||
double K36 = 39.54394;
|
||||
|
||||
double gain =
|
||||
0.25 *
|
||||
pow(10, -4); // changing exponenet drastically changes results of LQR
|
||||
|
||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
|
||||
Vars.LQRx =
|
||||
gain *
|
||||
((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 +
|
||||
K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) /
|
||||
-Vars.momentArm;
|
||||
Vars.LQRy =
|
||||
gain *
|
||||
((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 +
|
||||
K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) /
|
||||
-Vars.momentArm;
|
||||
|
||||
// LQR Force limiter X
|
||||
if (Vars.LQRx > Vars.thrust)
|
||||
Vars.LQRx = Vars.thrust;
|
||||
else if (Vars.LQRx < -1 * Vars.thrust)
|
||||
Vars.LQRx = -1 * Vars.thrust;
|
||||
|
||||
// LQR Force limiter Y
|
||||
if (Vars.LQRy > Vars.thrust)
|
||||
Vars.LQRy = Vars.thrust;
|
||||
else if (Vars.LQRy < -1 * Vars.thrust)
|
||||
Vars.LQRy = -1 * Vars.thrust;
|
||||
}
|
||||
|
||||
void TVC(struct sVars &Vars, double g) {
|
||||
if (Vars.thrust < 1) {
|
||||
// Define forces and moments for t = 0
|
||||
Vars.Fx = 0;
|
||||
Vars.Fy = 0;
|
||||
Vars.Fz = g * Vars.m0;
|
||||
|
||||
Vars.momentX = 0;
|
||||
Vars.momentY = 0;
|
||||
Vars.momentZ = 0;
|
||||
}
|
||||
|
||||
else {
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
Vars.xServoDegs = (180 / 3.1416) * asin(Vars.LQRx / Vars.thrust);
|
||||
|
||||
// Servo position limiter
|
||||
if (Vars.xServoDegs > Vars.maxServo)
|
||||
Vars.xServoDegs = Vars.maxServo;
|
||||
else if (Vars.xServoDegs < -1 * Vars.maxServo)
|
||||
Vars.xServoDegs = -1 * Vars.maxServo;
|
||||
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
Vars.yServoDegs = (180 / 3.1416) * asin(Vars.LQRy / Vars.thrust);
|
||||
|
||||
// Servo position limiter
|
||||
if (Vars.yServoDegs > Vars.maxServo)
|
||||
Vars.yServoDegs = Vars.maxServo;
|
||||
else if (Vars.yServoDegs < -1 * Vars.maxServo)
|
||||
Vars.yServoDegs = -1 * Vars.maxServo;
|
||||
|
||||
// Vector math to aqcuire thrust vector components
|
||||
Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (3.1416 / 180));
|
||||
Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (3.1416 / 180));
|
||||
Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) +
|
||||
(Vars.m * g);
|
||||
|
||||
// Calculate moment created by Fx and Fy
|
||||
Vars.momentX = Vars.Fx * Vars.momentArm;
|
||||
Vars.momentY = Vars.Fy * Vars.momentArm;
|
||||
Vars.momentZ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void vehicleDynamics(struct sVars &Vars, int t) {
|
||||
// Idot
|
||||
if (t < 1) {
|
||||
Vars.I11dot = 0;
|
||||
Vars.I22dot = 0;
|
||||
Vars.I33dot = 0;
|
||||
} else {
|
||||
Vars.I11dot = derivative(Vars.I11, Vars.I11prev, Vars.stepSize);
|
||||
Vars.I22dot = derivative(Vars.I22, Vars.I22prev, Vars.stepSize);
|
||||
Vars.I33dot = derivative(Vars.I33, Vars.I33prev, Vars.stepSize);
|
||||
}
|
||||
|
||||
// pdot, qdot, rdot
|
||||
Vars.yawddot = (Vars.momentX - Vars.I11dot * Vars.yawdot +
|
||||
Vars.I22 * Vars.pitchdot * Vars.rolldot -
|
||||
Vars.I33 * Vars.pitchdot * Vars.rolldot) /
|
||||
Vars.I11;
|
||||
Vars.pitchddot = (Vars.momentY - Vars.I22dot * Vars.pitchdot -
|
||||
Vars.I11 * Vars.rolldot * Vars.yawdot +
|
||||
Vars.I33 * Vars.rolldot * Vars.yawdot) /
|
||||
Vars.I22;
|
||||
Vars.rollddot = (Vars.momentZ - Vars.I33dot * Vars.rolldot +
|
||||
Vars.I11 * Vars.pitchdot * Vars.yawdot -
|
||||
Vars.I22 * Vars.pitchdot * Vars.yawdot) /
|
||||
Vars.I33;
|
||||
|
||||
if (t < 1) {
|
||||
Vars.x = 0;
|
||||
Vars.y = 0;
|
||||
|
||||
Vars.ax = 0;
|
||||
Vars.ay = 0;
|
||||
Vars.az = Vars.Fz / Vars.m0;
|
||||
}
|
||||
|
||||
else {
|
||||
// p, q, r
|
||||
Vars.yawdot = integral(Vars.yawddot, Vars.yawdotPrev, Vars.stepSize);
|
||||
Vars.pitchdot = integral(Vars.pitchddot, Vars.pitchdotPrev, Vars.stepSize);
|
||||
Vars.rolldot = integral(Vars.rollddot, Vars.rolldotPrev, Vars.stepSize);
|
||||
|
||||
// ax ay az
|
||||
Vars.ax =
|
||||
(Vars.Fx / Vars.m) + (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy);
|
||||
Vars.ay =
|
||||
(Vars.Fy / Vars.m) + (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot);
|
||||
Vars.az =
|
||||
(Vars.Fz / Vars.m) + (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx);
|
||||
|
||||
// vx vy vz in Body frame
|
||||
Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize);
|
||||
Vars.vy = integral(Vars.ay, Vars.vyPrev, Vars.stepSize);
|
||||
Vars.vz = integral(Vars.az, Vars.vzPrev, Vars.stepSize);
|
||||
|
||||
// Xe
|
||||
Vars.x = integral(Vars.vx, Vars.xPrev, Vars.stepSize);
|
||||
Vars.y = integral(Vars.vy, Vars.yPrev, Vars.stepSize);
|
||||
Vars.z = integral(Vars.vz, Vars.zPrev, Vars.stepSize);
|
||||
|
||||
// Euler Angles
|
||||
Vars.phidot = Vars.yawdot + (Vars.pitchdot * sin(Vars.yaw) +
|
||||
Vars.rolldot * cos(Vars.yaw)) *
|
||||
(sin(Vars.pitch) / cos(Vars.pitch));
|
||||
Vars.thetadot =
|
||||
Vars.pitchdot * cos(Vars.yaw) - Vars.rolldot * sin(Vars.pitch);
|
||||
Vars.psidot =
|
||||
(Vars.pitchdot * sin(Vars.yaw) + Vars.rolldot * cos(Vars.yaw)) /
|
||||
cos(Vars.pitch);
|
||||
|
||||
Vars.yaw = integral(Vars.phidot, Vars.yawPrev, Vars.stepSize);
|
||||
Vars.pitch = integral(Vars.thetadot, Vars.pitchPrev, Vars.stepSize);
|
||||
Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize);
|
||||
}
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
Vars.I11prev = Vars.I11;
|
||||
Vars.I22prev = Vars.I22;
|
||||
Vars.I33prev = Vars.I33;
|
||||
|
||||
Vars.yawPrev = Vars.yaw;
|
||||
Vars.pitchPrev = Vars.pitch;
|
||||
Vars.rollPrev = Vars.roll;
|
||||
|
||||
Vars.yawdotPrev = Vars.yawdot;
|
||||
Vars.pitchdotPrev = Vars.pitchdot;
|
||||
Vars.rolldotPrev = Vars.rolldot;
|
||||
|
||||
Vars.yawddotPrev = Vars.yawddot;
|
||||
Vars.pitchddotPrev = Vars.pitchddot;
|
||||
Vars.rollddotPrev = Vars.rollddot;
|
||||
|
||||
Vars.axPrev = Vars.ax;
|
||||
Vars.ayPrev = Vars.ay;
|
||||
Vars.azPrev = Vars.az;
|
||||
|
||||
Vars.vxPrev = Vars.vx;
|
||||
Vars.vyPrev = Vars.vy;
|
||||
Vars.vzPrev = Vars.vz;
|
||||
|
||||
Vars.xPrev = Vars.x;
|
||||
Vars.yPrev = Vars.y;
|
||||
Vars.zPrev = Vars.z;
|
||||
}
|
||||
|
||||
void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
|
||||
// writing to output file
|
||||
outfile << t << ", ";
|
||||
|
||||
outfile << Vars.x << ", ";
|
||||
outfile << Vars.y << ", ";
|
||||
outfile << Vars.z << ", ";
|
||||
|
||||
outfile << Vars.vx << ", ";
|
||||
outfile << Vars.vy << ", ";
|
||||
outfile << Vars.vz << ", ";
|
||||
|
||||
outfile << Vars.ax << ", ";
|
||||
outfile << Vars.ay << ", ";
|
||||
outfile << Vars.az << ", ";
|
||||
|
||||
outfile << Vars.yaw * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.pitch * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.roll * 180 / 3.1416 << ", ";
|
||||
|
||||
outfile << Vars.yawdot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.pitchdot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.rolldot * 180 / 3.1416 << ", ";
|
||||
|
||||
outfile << Vars.yawddot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.pitchddot * 180 / 3.1416 << ", ";
|
||||
outfile << Vars.rollddot * 180 / 3.1416 << ", ";
|
||||
|
||||
outfile << Vars.I11 << ", ";
|
||||
outfile << Vars.I22 << ", ";
|
||||
outfile << Vars.I33 << ", ";
|
||||
|
||||
outfile << Vars.I11dot << ", ";
|
||||
outfile << Vars.I22dot << ", ";
|
||||
outfile << Vars.I33dot << ", ";
|
||||
|
||||
outfile << Vars.xServoDegs << ", ";
|
||||
outfile << Vars.yServoDegs << ", ";
|
||||
|
||||
outfile << Vars.m << ", ";
|
||||
outfile << Vars.thrust << ", ";
|
||||
outfile << Vars.burnElapsed << ", ";
|
||||
outfile << Vars.Fz << ", ";
|
||||
|
||||
outfile << Vars.LQRx << ", ";
|
||||
outfile << Vars.LQRy << std::endl;
|
||||
}
|
||||
|
||||
double derivative(double x2, double x1, double dt) {
|
||||
double dxdt = (x2 - x1) / (dt / 1000);
|
||||
return dxdt;
|
||||
}
|
||||
|
||||
double integral(double x, double y, double dt) {
|
||||
double integ = (x * dt / 1000) + y;
|
||||
return integ;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user