From 24bf3f210234bebc40492b342833ea83d7d328f5 Mon Sep 17 00:00:00 2001 From: Anson Date: Sun, 12 Sep 2021 17:34:40 -0700 Subject: [PATCH] reformatted code --- include/sVars.h | 57 ++-- src/main.cpp | 67 +++-- src/sim.cpp | 686 ++++++++++++++++++++++++------------------------ 3 files changed, 402 insertions(+), 408 deletions(-) diff --git a/include/sVars.h b/include/sVars.h index cddd881..e1649d1 100644 --- a/include/sVars.h +++ b/include/sVars.h @@ -3,41 +3,40 @@ #ifndef SVARS_H #define SVARS_H -struct sVars -{ - double x, y, z; - double xPrev, yPrev, zPrev; - double vx, vy, vz; - double vxPrev, vyPrev, vzPrev; - double vxBody, vyBody, vzBody; - double ax, ay, az, axPrev, ayPrev, azPrev; +struct sVars { + double x, y, z; + double xPrev, yPrev, zPrev; + double vx, vy, vz; + double vxPrev, vyPrev, vzPrev; + double vxBody, vyBody, vzBody; + double ax, ay, az, axPrev, ayPrev, azPrev; - double yaw, pitch, roll; - double phidot, thetadot, psidot; - double yawPrev, pitchPrev, rollPrev; - double yawdot, pitchdot, rolldot; - double yawdotPrev, pitchdotPrev, rolldotPrev; - double yawddot, pitchddot, rollddot; - double yawddotPrev, pitchddotPrev, rollddotPrev; + double yaw, pitch, roll; + double phidot, thetadot, psidot; + double yawPrev, pitchPrev, rollPrev; + double yawdot, pitchdot, rolldot; + double yawdotPrev, pitchdotPrev, rolldotPrev; + double yawddot, pitchddot, rollddot; + double yawddotPrev, pitchddotPrev, rollddotPrev; - double m, m0, mp, mb, mdot; - double vehicleHeight, vehicleRadius, momentArm; + double m, m0, mp, mb, mdot; + double vehicleHeight, vehicleRadius, momentArm; - double tb; - double vb; - double thrust, thrust_prev, burnElapsed, burnStart; - double LQRx, LQRy, Fx, Fy, Fz; - double momentX, momentY, momentZ; + double tb; + double vb; + double thrust, thrust_prev, burnElapsed, burnStart; + double LQRx, LQRy, Fx, Fy, Fz; + double momentX, momentY, momentZ; - double I11, I22, I33; - double I11prev, I22prev, I33prev; - double I11dot, I22dot, I33dot; + double I11, I22, I33; + double I11prev, I22prev, I33prev; + double I11dot, I22dot, I33dot; - int maxServo; - double xServoDegs, yServoDegs; + int maxServo; + double xServoDegs, yServoDegs; - double simTime; - int stepSize; + double simTime; + int stepSize; }; #endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 65c099a..3dbaae0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,50 +1,49 @@ -#include "sim.h" #include "sVars.h" +#include "sim.h" #include void sim(struct sVars &); -int main() -{ - sVars Vars; +int main() { + sVars Vars; - // Initial Velocity - Vars.vx = 0; // [m/s] - Vars.vy = 0; // [m/s] - Vars.vz = 0; // [m/s] + // Initial Velocity + Vars.vx = 0; // [m/s] + Vars.vy = 0; // [m/s] + Vars.vz = 0; // [m/s] - // Initial YPR - Vars.yaw = 0 * 3.1416 / 180; // [rad] - Vars.pitch = 0 * 3.1416 / 180; // [rad] - Vars.roll = 0 * 3.1416 / 180; // [rad] + // Initial YPR + Vars.yaw = 0 * 3.1416 / 180; // [rad] + Vars.pitch = 0 * 3.1416 / 180; // [rad] + Vars.roll = 0 * 3.1416 / 180; // [rad] - // Initial YPRdot - Vars.yawdot = 0 * 3.1416 / 180; // [rad/s] - Vars.pitchdot = 0 * 3.1416 / 180; // [rad/s] - Vars.rolldot = 0 * 3.1416 / 180; // [rad/s] + // Initial YPRdot + Vars.yawdot = 0 * 3.1416 / 180; // [rad/s] + Vars.pitchdot = 0 * 3.1416 / 180; // [rad/s] + Vars.rolldot = 0 * 3.1416 / 180; // [rad/s] - // Servo Limitation - Vars.maxServo = 15; // [degs] + // Servo Limitation + Vars.maxServo = 15; // [degs] - // Vehicle Properties - Vars.m0 = 1.2; // [kg] - Vars.vehicleHeight = 0.5318; // [m] - Vars.vehicleRadius = 0.05105; // [m] - Vars.momentArm = 0.145; // [m] + // Vehicle Properties + Vars.m0 = 1.2; // [kg] + Vars.vehicleHeight = 0.5318; // [m] + Vars.vehicleRadius = 0.05105; // [m] + Vars.momentArm = 0.145; // [m] - // Sim Step Size - Vars.stepSize = 1; // [ms] + // Sim Step Size + Vars.stepSize = 1; // [ms] - // Other Properties - Vars.mp = 0.06; // [kg] - Vars.mb = Vars.m0 - Vars.mp; // [kg] - Vars.tb = 3.45 - 0.148; // [s] - Vars.mdot = Vars.mp / Vars.tb; // [kg/s] + // Other Properties + Vars.mp = 0.06; // [kg] + Vars.mb = Vars.m0 - Vars.mp; // [kg] + Vars.tb = 3.45 - 0.148; // [s] + Vars.mdot = Vars.mp / Vars.tb; // [kg/s] - sim(Vars); + sim(Vars); - std::cout << "Finished"; - std::cin.get(); + std::cout << "Finished"; + std::cin.get(); - return 0; + return 0; } \ No newline at end of file diff --git a/src/sim.cpp b/src/sim.cpp index e2ee7dd..b3920ed 100644 --- a/src/sim.cpp +++ b/src/sim.cpp @@ -4,433 +4,429 @@ #include #include -void sim(struct sVars &Vars) -{ - double g = -9.81; +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; + // defining a few random values here cause I'm lazy + Vars.burnElapsed = 2000; + Vars.m = Vars.m0; + Vars.thrust_prev = 0; - burnStartTimeCalc(Vars, g); + burnStartTimeCalc(Vars, g); - // Deleting any previous output file - if (remove("simOut.csv") != 0) - perror("Error deleting file"); - else - puts("File successfully deleted"); + // 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); + // 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; + // 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); - } + // 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(); + 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; +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; + 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; + 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 > 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; + else if ((i > 3.382) & (i < 3.46)) + thrust = -195.78 * i + 675.11; - v = (((thrust / m) + g) * dt) + v; - h = v * dt + h; - } + 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 + 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; + 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 +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); + // 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); - } + 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 if (b_burnStart) { + // Start burn + Vars.burnStart = t; + Vars.burnElapsed = 0; + } - else - Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn + 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; + 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 > 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; + else if ((Vars.burnElapsed > 3.382) & (Vars.burnElapsed < 3.46)) + Vars.thrust = -195.78 * Vars.burnElapsed + 675.11; } -void lqrCalc(struct sVars &Vars) -{ +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); + 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 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 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 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 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; - 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)); + // 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; - // 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; + 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; - // 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; + // 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; - 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; + // 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; +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; - } + 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); + 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; + // 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); + // 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; + // 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); + // 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; - } + // 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); - } +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; + // 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; + if (t < 1) { + Vars.x = 0; + Vars.y = 0; - Vars.ax = 0; - Vars.ay = 0; - Vars.az = Vars.Fz / Vars.m0; - } + 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); + 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); + // 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); + // 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); + // 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); + // 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); - } + 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; + // 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.yawPrev = Vars.yaw; + Vars.pitchPrev = Vars.pitch; + Vars.rollPrev = Vars.roll; - Vars.yawdotPrev = Vars.yawdot; - Vars.pitchdotPrev = Vars.pitchdot; - Vars.rolldotPrev = Vars.rolldot; + 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.yawddotPrev = Vars.yawddot; + Vars.pitchddotPrev = Vars.pitchddot; + Vars.rollddotPrev = Vars.rollddot; - Vars.axPrev = Vars.ax; - Vars.ayPrev = Vars.ay; - Vars.azPrev = Vars.az; + 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.vxPrev = Vars.vx; + Vars.vyPrev = Vars.vy; + Vars.vzPrev = Vars.vz; - Vars.xPrev = Vars.x; - Vars.yPrev = Vars.y; - Vars.zPrev = Vars.z; + 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 << ", "; +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.x << ", "; + outfile << Vars.y << ", "; + outfile << Vars.z << ", "; - outfile << Vars.vx << ", "; - outfile << Vars.vy << ", "; - outfile << Vars.vz << ", "; + outfile << Vars.vx << ", "; + outfile << Vars.vy << ", "; + outfile << Vars.vz << ", "; - outfile << Vars.ax << ", "; - outfile << Vars.ay << ", "; - outfile << Vars.az << ", "; + 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.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.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.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.I11 << ", "; + outfile << Vars.I22 << ", "; + outfile << Vars.I33 << ", "; - outfile << Vars.I11dot << ", "; - outfile << Vars.I22dot << ", "; - outfile << Vars.I33dot << ", "; + outfile << Vars.I11dot << ", "; + outfile << Vars.I22dot << ", "; + outfile << Vars.I33dot << ", "; - outfile << Vars.xServoDegs << ", "; - outfile << Vars.yServoDegs << ", "; + outfile << Vars.xServoDegs << ", "; + outfile << Vars.yServoDegs << ", "; - outfile << Vars.m << ", "; - outfile << Vars.thrust << ", "; - outfile << Vars.burnElapsed << ", "; - outfile << Vars.Fz << ", "; + outfile << Vars.m << ", "; + outfile << Vars.thrust << ", "; + outfile << Vars.burnElapsed << ", "; + outfile << Vars.Fz << ", "; - outfile << Vars.LQRx << ", "; - outfile << Vars.LQRy << std::endl; + 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 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; +double integral(double x, double y, double dt) { + double integ = (x * dt / 1000) + y; + return integ; } \ No newline at end of file