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

fixes #1

Browse Source
This commit is contained in:
Anson Biggs 2021-09-13 13:11:47 -07:00
parent bb31512572
commit 9f5cd7bdf7
4 changed files with 113 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
build/debug/gainCalc.m
View File

@ -37,24 +37,24 @@ K = Rinv * B' * S;
%% Outputs %% Outputs
% Copy results in command window to LQRcalc function in C++ % Copy results in command window to LQRcalc function in C++
fprintf("float K11 = %3.5f;\n", K(1, 1)) fprintf("double K11 = %3.5f;\n", K(1, 1))
fprintf("float K12 = %3.5f;\n", K(1, 2)) fprintf("double K12 = %3.5f;\n", K(1, 2))
fprintf("float K13 = %3.5f;\n", K(1, 3)) fprintf("double K13 = %3.5f;\n", K(1, 3))
fprintf("float K14 = %3.5f;\n", K(1, 4)) fprintf("double K14 = %3.5f;\n", K(1, 4))
fprintf("float K15 = %3.5f;\n", K(1, 5)) fprintf("double K15 = %3.5f;\n", K(1, 5))
fprintf("float K16 = %3.5f;\n", K(1, 6)) fprintf("double K16 = %3.5f;\n", K(1, 6))
fprintf("float K21 = %3.5f;\n", K(2, 1)) fprintf("double K21 = %3.5f;\n", K(2, 1))
fprintf("float K22 = %3.5f;\n", K(2, 2)) fprintf("double K22 = %3.5f;\n", K(2, 2))
fprintf("float K23 = %3.5f;\n", K(2, 3)) fprintf("double K23 = %3.5f;\n", K(2, 3))
fprintf("float K24 = %3.5f;\n", K(2, 4)) fprintf("double K24 = %3.5f;\n", K(2, 4))
fprintf("float K25 = %3.5f;\n", K(2, 5)) fprintf("double K25 = %3.5f;\n", K(2, 5))
fprintf("float K26 = %3.5f;\n", K(2, 6)) fprintf("double K26 = %3.5f;\n", K(2, 6))
fprintf("float K31 = %3.5f;\n", K(3, 1)) fprintf("double K31 = %3.5f;\n", K(3, 1))
fprintf("float K32 = %3.5f;\n", K(3, 2)) fprintf("double K32 = %3.5f;\n", K(3, 2))
fprintf("float K33 = %3.5f;\n", K(3, 3)) fprintf("double K33 = %3.5f;\n", K(3, 3))
fprintf("float K34 = %3.5f;\n", K(3, 4)) fprintf("double K34 = %3.5f;\n", K(3, 4))
fprintf("float K35 = %3.5f;\n", K(3, 5)) fprintf("double K35 = %3.5f;\n", K(3, 5))
fprintf("float K36 = %3.5f;\n", K(3, 6)) fprintf("double K36 = %3.5f;\n", K(3, 6))
syms yaw pitch roll yawdot pitchdot rolldot gain syms yaw pitch roll yawdot pitchdot rolldot gain
w = [0.5*yaw 0.5*pitch 0.5*roll yawdot pitchdot rolldot]'; w = [0.5*yaw 0.5*pitch 0.5*roll yawdot pitchdot rolldot]';

36
build/release/gainCalc.m
View File

@ -37,24 +37,24 @@ K = Rinv * B' * S;
%% Outputs %% Outputs
% Copy results in command window to LQRcalc function in C++ % Copy results in command window to LQRcalc function in C++
fprintf("float K11 = %3.5f;\n", K(1, 1)) fprintf("double K11 = %3.5f;\n", K(1, 1))
fprintf("float K12 = %3.5f;\n", K(1, 2)) fprintf("double K12 = %3.5f;\n", K(1, 2))
fprintf("float K13 = %3.5f;\n", K(1, 3)) fprintf("double K13 = %3.5f;\n", K(1, 3))
fprintf("float K14 = %3.5f;\n", K(1, 4)) fprintf("double K14 = %3.5f;\n", K(1, 4))
fprintf("float K15 = %3.5f;\n", K(1, 5)) fprintf("double K15 = %3.5f;\n", K(1, 5))
fprintf("float K16 = %3.5f;\n", K(1, 6)) fprintf("double K16 = %3.5f;\n", K(1, 6))
fprintf("float K21 = %3.5f;\n", K(2, 1)) fprintf("double K21 = %3.5f;\n", K(2, 1))
fprintf("float K22 = %3.5f;\n", K(2, 2)) fprintf("double K22 = %3.5f;\n", K(2, 2))
fprintf("float K23 = %3.5f;\n", K(2, 3)) fprintf("double K23 = %3.5f;\n", K(2, 3))
fprintf("float K24 = %3.5f;\n", K(2, 4)) fprintf("double K24 = %3.5f;\n", K(2, 4))
fprintf("float K25 = %3.5f;\n", K(2, 5)) fprintf("double K25 = %3.5f;\n", K(2, 5))
fprintf("float K26 = %3.5f;\n", K(2, 6)) fprintf("double K26 = %3.5f;\n", K(2, 6))
fprintf("float K31 = %3.5f;\n", K(3, 1)) fprintf("double K31 = %3.5f;\n", K(3, 1))
fprintf("float K32 = %3.5f;\n", K(3, 2)) fprintf("double K32 = %3.5f;\n", K(3, 2))
fprintf("float K33 = %3.5f;\n", K(3, 3)) fprintf("double K33 = %3.5f;\n", K(3, 3))
fprintf("float K34 = %3.5f;\n", K(3, 4)) fprintf("double K34 = %3.5f;\n", K(3, 4))
fprintf("float K35 = %3.5f;\n", K(3, 5)) fprintf("double K35 = %3.5f;\n", K(3, 5))
fprintf("float K36 = %3.5f;\n", K(3, 6)) fprintf("double K36 = %3.5f;\n", K(3, 6))
syms yaw pitch roll yawdot pitchdot rolldot gain syms yaw pitch roll yawdot pitchdot rolldot gain
w = [0.5*yaw 0.5*pitch 0.5*roll yawdot pitchdot rolldot]'; w = [0.5*yaw 0.5*pitch 0.5*roll yawdot pitchdot rolldot]';

50
include/sVars.h
View File

@ -4,38 +4,38 @@
#define SVARS_H #define SVARS_H
struct sVars { struct sVars {
float x, y, z; double x, y, z;
float xPrev, yPrev, zPrev; double xPrev, yPrev, zPrev;
float vx, vy, vz; double vx, vy, vz;
float vxPrev, vyPrev, vzPrev; double vxPrev, vyPrev, vzPrev;
float vxBody, vyBody, vzBody; double vxBody, vyBody, vzBody;
float ax, ay, az, axPrev, ayPrev, azPrev; double ax, ay, az, axPrev, ayPrev, azPrev;
float yaw, pitch, roll; double yaw, pitch, roll;
float phidot, thetadot, psidot; double phidot, thetadot, psidot;
float yawPrev, pitchPrev, rollPrev; double yawPrev, pitchPrev, rollPrev;
float yawdot, pitchdot, rolldot; double yawdot, pitchdot, rolldot;
float yawdotPrev, pitchdotPrev, rolldotPrev; double yawdotPrev, pitchdotPrev, rolldotPrev;
float yawddot, pitchddot, rollddot; double yawddot, pitchddot, rollddot;
float yawddotPrev, pitchddotPrev, rollddotPrev; double yawddotPrev, pitchddotPrev, rollddotPrev;
float m, m0, mp, mb, mdot; double m, m0, mp, mb, mdot;
float vehicleHeight, vehicleRadius, momentArm; double vehicleHeight, vehicleRadius, momentArm;
float tb; double tb;
float vb; double vb;
float thrust, thrust_prev, burnElapsed, burnStart; double thrust, thrust_prev, burnElapsed, burnStart;
float LQRx, LQRy, Fx, Fy, Fz; double LQRx, LQRy, Fx, Fy, Fz;
float momentX, momentY, momentZ; double momentX, momentY, momentZ;
float I11, I22, I33; double I11, I22, I33;
float I11prev, I22prev, I33prev; double I11prev, I22prev, I33prev;
float I11dot, I22dot, I33dot; double I11dot, I22dot, I33dot;
int maxServo; int maxServo;
float xServoDegs, yServoDegs; double xServoDegs, yServoDegs;
float simTime; double simTime;
int stepSize; int stepSize;
}; };

104
include/sim.h
View File

@ -6,13 +6,13 @@ void lqrCalc(struct sVars &);
void TVC(struct sVars &); void TVC(struct sVars &);
void vehicleDynamics(struct sVars &, int t); void vehicleDynamics(struct sVars &, int t);
void write2CSV(struct sVars &, std::fstream &outfile, int t); void write2CSV(struct sVars &, std::fstream &outfile, int t);
float derivative(float x2, float x1, float dt); double derivative(double x2, double x1, double dt);
float integral(float x2, float x1, float dt); double integral(double x2, double x1, double dt);
// Any parameters that are constants should be declared here instead of buried // Any parameters that are constants should be declared here instead of buried
// in code // in code
float const dt = 0.001; double const dt = 0.001;
float const g = -9.81; double const g = -9.81;
void sim(struct sVars &Vars) { void sim(struct sVars &Vars) {
@ -55,12 +55,12 @@ void sim(struct sVars &Vars) {
} }
void burnStartTimeCalc(struct sVars &Vars) { void burnStartTimeCalc(struct sVars &Vars) {
float v = Vars.vz; double v = Vars.vz;
float h = 0; double h = 0;
float a, j, m, thrust; double a, j, m, thrust;
for (float i = 0.148; i < 3.450; i = i + dt) { for (double i = 0.148; i < 3.450; i = i + dt) {
m = Vars.m0 - i * Vars.mdot; m = Vars.m0 - i * Vars.mdot;
if ((i > 0.147) && (i < 0.420)) if ((i > 0.147) && (i < 0.420))
@ -80,12 +80,12 @@ void burnStartTimeCalc(struct sVars &Vars) {
Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height
Vars.vb = v; // terminal velocity Vars.vb = v; // terminal velocity
float burnStartTime = Vars.vb / -g; double burnStartTime = Vars.vb / -g;
Vars.simTime = (Vars.tb + burnStartTime) * 1000; Vars.simTime = (Vars.tb + burnStartTime) * 1000;
} }
void thrustSelection(struct sVars &Vars, int t) { void thrustSelection(struct sVars &Vars, int t) {
float tol = 0.001; // 0.001 seems to be a nice tolerance 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 // Check to see if current velocity is close to the F15's total velocity
bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) & bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) &
@ -130,23 +130,23 @@ void lqrCalc(struct sVars &Vars) {
/* /*
float n = sqrt(398600 / pow(6678, 3)); double n = sqrt(398600 / pow(6678, 3));
float k1 = (Vars.I22 - Vars.I33) / Vars.I11; double k1 = (Vars.I22 - Vars.I33) / Vars.I11;
float k2 = (Vars.I11 - Vars.I33) / Vars.I22; double k2 = (Vars.I11 - Vars.I33) / Vars.I22;
float k3 = (Vars.I22 - Vars.I11) / Vars.I33; double k3 = (Vars.I22 - Vars.I11) / Vars.I33;
float R11 = pow(10, 2); double R11 = pow(10, 2);
float R22 = pow(10, 2); double R22 = pow(10, 2);
float R33 = pow(10, 2); double R33 = pow(10, 2);
float F11 = -2 * pow(n, 2) * 4 * k1; double F11 = -2 * pow(n, 2) * 4 * k1;
float F22 = -2 * pow(n, 2) * 3 * k2; double F22 = -2 * pow(n, 2) * 3 * k2;
float F33 = -2 * pow(n, 2) * k3; double F33 = -2 * pow(n, 2) * k3;
float G31 = n * (1 - k1); double G31 = n * (1 - k1);
float G13 = n * (k3 - 1); double G13 = n * (k3 - 1);
float d = 0.5; double d = 0.5;
/* /*
The following calculations are based on output of LQR.m to avoid working 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. with matrices in C++ Please see .m file for details on calculation.
@ -154,27 +154,27 @@ void lqrCalc(struct sVars &Vars) {
https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
*/ */
/* /*
float gain = 0.25; double gain = 0.25;
float K11 = double K11 =
-1 * gain * -1 * gain *
(Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + (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) / R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) /
(R11 * d * Vars.I11 * (R11 * d * Vars.I11 *
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1)); (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
float K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22); double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22);
float K33 = double K33 =
-1 * gain * -1 * gain *
(Vars.I33 * (Vars.I33 *
(Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + (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)) / R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) /
(R33 * d * pow(abs(Vars.I33), 2) * (R33 * d * pow(abs(Vars.I33), 2) *
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1)); (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
float K34 = double K34 =
gain * (R11 * pow(abs(Vars.I11), 2) * G13) / gain * (R11 * pow(abs(Vars.I11), 2) * G13) /
(R33 * Vars.I33 * (R33 * Vars.I33 *
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1)); (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
float K16 = double K16 =
gain * (R33 * pow(abs(Vars.I33), 2) * G31) / gain * (R33 * pow(abs(Vars.I33), 2) * G31) /
(R11 * Vars.I11 * (R11 * Vars.I11 *
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1)); (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
@ -186,26 +186,26 @@ void lqrCalc(struct sVars &Vars) {
*/ */
// Paste in Values from gainCalc.m // Paste in Values from gainCalc.m
float K11 = 39.54316; double K11 = 39.54316;
float K12 = 0.00000; double K12 = 0.00000;
float K13 = -0.00000; double K13 = -0.00000;
float K14 = 39.55769; double K14 = 39.55769;
float K15 = 0.00000; double K15 = 0.00000;
float K16 = 0.00000; double K16 = 0.00000;
float K21 = 0.00000; double K21 = 0.00000;
float K22 = 39.54316; double K22 = 39.54316;
float K23 = 0.00000; double K23 = 0.00000;
float K24 = 0.00000; double K24 = 0.00000;
float K25 = 39.55769; double K25 = 39.55769;
float K26 = 0.00000; double K26 = 0.00000;
float K31 = 0.00000; double K31 = 0.00000;
float K32 = 0.00000; double K32 = 0.00000;
float K33 = 39.54316; double K33 = 39.54316;
float K34 = 0.00000; double K34 = 0.00000;
float K35 = 0.00000; double K35 = 0.00000;
float K36 = 39.54394; double K36 = 39.54394;
float gain = double gain =
0.25 * 0.25 *
pow(10, -4); // changing exponenet drastically changes results of LQR pow(10, -4); // changing exponenet drastically changes results of LQR
@ -430,12 +430,12 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
outfile << Vars.LQRy << std::endl; outfile << Vars.LQRy << std::endl;
} }
float derivative(float x2, float x1, float dt) { double derivative(double x2, double x1, double dt) {
float dxdt = (x2 - x1) / (dt / 1000); double dxdt = (x2 - x1) / (dt / 1000);
return dxdt; return dxdt;
} }
float integral(float x, float y, float dt) { double integral(double x, double y, double dt) {
float integ = (x * dt / 1000) + y; double integ = (x * dt / 1000) + y;
return integ; return integ;
} }