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

closes #1

Browse Source
This commit is contained in:
Anson Biggs 2021-09-13 09:09:46 -07:00
parent b94568bea8
commit bb31512572
9 changed files with 4412 additions and 4412 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
% Copy results in command window to LQRcalc function in C++
fprintf("double K11 = %3.5f;\n", K(1, 1))
fprintf("double K12 = %3.5f;\n", K(1, 2))
fprintf("double K13 = %3.5f;\n", K(1, 3))
fprintf("double K14 = %3.5f;\n", K(1, 4))
fprintf("double K15 = %3.5f;\n", K(1, 5))
fprintf("double K16 = %3.5f;\n", K(1, 6))
fprintf("double K21 = %3.5f;\n", K(2, 1))
fprintf("double K22 = %3.5f;\n", K(2, 2))
fprintf("double K23 = %3.5f;\n", K(2, 3))
fprintf("double K24 = %3.5f;\n", K(2, 4))
fprintf("double K25 = %3.5f;\n", K(2, 5))
fprintf("double K26 = %3.5f;\n", K(2, 6))
fprintf("double K31 = %3.5f;\n", K(3, 1))
fprintf("double K32 = %3.5f;\n", K(3, 2))
fprintf("double K33 = %3.5f;\n", K(3, 3))
fprintf("double K34 = %3.5f;\n", K(3, 4))
fprintf("double K35 = %3.5f;\n", K(3, 5))
fprintf("double K36 = %3.5f;\n", K(3, 6))
fprintf("float K11 = %3.5f;\n", K(1, 1))
fprintf("float K12 = %3.5f;\n", K(1, 2))
fprintf("float K13 = %3.5f;\n", K(1, 3))
fprintf("float K14 = %3.5f;\n", K(1, 4))
fprintf("float K15 = %3.5f;\n", K(1, 5))
fprintf("float K16 = %3.5f;\n", K(1, 6))
fprintf("float K21 = %3.5f;\n", K(2, 1))
fprintf("float K22 = %3.5f;\n", K(2, 2))
fprintf("float K23 = %3.5f;\n", K(2, 3))
fprintf("float K24 = %3.5f;\n", K(2, 4))
fprintf("float K25 = %3.5f;\n", K(2, 5))
fprintf("float K26 = %3.5f;\n", K(2, 6))
fprintf("float K31 = %3.5f;\n", K(3, 1))
fprintf("float K32 = %3.5f;\n", K(3, 2))
fprintf("float K33 = %3.5f;\n", K(3, 3))
fprintf("float K34 = %3.5f;\n", K(3, 4))
fprintf("float K35 = %3.5f;\n", K(3, 5))
fprintf("float K36 = %3.5f;\n", K(3, 6))
syms yaw pitch roll yawdot pitchdot rolldot gain
w = [0.5*yaw 0.5*pitch 0.5*roll yawdot pitchdot rolldot]';

BIN
build/debug/main.exe
View File

Binary file not shown.

BIN
build/debug/outputs/Accel-Vel-Alt vs Time.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 39 KiB

After

Width:  |  Height:  |  Size: 39 KiB

BIN
build/debug/outputs/Euler Angles vs Time.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 33 KiB

After

Width:  |  Height:  |  Size: 33 KiB

BIN
build/debug/outputs/Servo Position vs Time.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 34 KiB

After

Width:  |  Height:  |  Size: 31 KiB

8602
build/debug/simOut.csv
View File

File diff suppressed because it is too large Load Diff

36
build/release/gainCalc.m
View File

@ -37,24 +37,24 @@ K = Rinv * B' * S;
%% Outputs
% Copy results in command window to LQRcalc function in C++
fprintf("double K11 = %3.5f;\n", K(1, 1))
fprintf("double K12 = %3.5f;\n", K(1, 2))
fprintf("double K13 = %3.5f;\n", K(1, 3))
fprintf("double K14 = %3.5f;\n", K(1, 4))
fprintf("double K15 = %3.5f;\n", K(1, 5))
fprintf("double K16 = %3.5f;\n", K(1, 6))
fprintf("double K21 = %3.5f;\n", K(2, 1))
fprintf("double K22 = %3.5f;\n", K(2, 2))
fprintf("double K23 = %3.5f;\n", K(2, 3))
fprintf("double K24 = %3.5f;\n", K(2, 4))
fprintf("double K25 = %3.5f;\n", K(2, 5))
fprintf("double K26 = %3.5f;\n", K(2, 6))
fprintf("double K31 = %3.5f;\n", K(3, 1))
fprintf("double K32 = %3.5f;\n", K(3, 2))
fprintf("double K33 = %3.5f;\n", K(3, 3))
fprintf("double K34 = %3.5f;\n", K(3, 4))
fprintf("double K35 = %3.5f;\n", K(3, 5))
fprintf("double K36 = %3.5f;\n", K(3, 6))
fprintf("float K11 = %3.5f;\n", K(1, 1))
fprintf("float K12 = %3.5f;\n", K(1, 2))
fprintf("float K13 = %3.5f;\n", K(1, 3))
fprintf("float K14 = %3.5f;\n", K(1, 4))
fprintf("float K15 = %3.5f;\n", K(1, 5))
fprintf("float K16 = %3.5f;\n", K(1, 6))
fprintf("float K21 = %3.5f;\n", K(2, 1))
fprintf("float K22 = %3.5f;\n", K(2, 2))
fprintf("float K23 = %3.5f;\n", K(2, 3))
fprintf("float K24 = %3.5f;\n", K(2, 4))
fprintf("float K25 = %3.5f;\n", K(2, 5))
fprintf("float K26 = %3.5f;\n", K(2, 6))
fprintf("float K31 = %3.5f;\n", K(3, 1))
fprintf("float K32 = %3.5f;\n", K(3, 2))
fprintf("float K33 = %3.5f;\n", K(3, 3))
fprintf("float K34 = %3.5f;\n", K(3, 4))
fprintf("float K35 = %3.5f;\n", K(3, 5))
fprintf("float K36 = %3.5f;\n", K(3, 6))
syms yaw pitch roll yawdot pitchdot rolldot gain
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
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;
float x, y, z;
float xPrev, yPrev, zPrev;
float vx, vy, vz;
float vxPrev, vyPrev, vzPrev;
float vxBody, vyBody, vzBody;
float 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;
float yaw, pitch, roll;
float phidot, thetadot, psidot;
float yawPrev, pitchPrev, rollPrev;
float yawdot, pitchdot, rolldot;
float yawdotPrev, pitchdotPrev, rolldotPrev;
float yawddot, pitchddot, rollddot;
float yawddotPrev, pitchddotPrev, rollddotPrev;
double m, m0, mp, mb, mdot;
double vehicleHeight, vehicleRadius, momentArm;
float m, m0, mp, mb, mdot;
float vehicleHeight, vehicleRadius, momentArm;
double tb;
double vb;
double thrust, thrust_prev, burnElapsed, burnStart;
double LQRx, LQRy, Fx, Fy, Fz;
double momentX, momentY, momentZ;
float tb;
float vb;
float thrust, thrust_prev, burnElapsed, burnStart;
float LQRx, LQRy, Fx, Fy, Fz;
float momentX, momentY, momentZ;
double I11, I22, I33;
double I11prev, I22prev, I33prev;
double I11dot, I22dot, I33dot;
float I11, I22, I33;
float I11prev, I22prev, I33prev;
float I11dot, I22dot, I33dot;
int maxServo;
double xServoDegs, yServoDegs;
float xServoDegs, yServoDegs;
double simTime;
float simTime;
int stepSize;
};

100
include/sim.h
View File

@ -6,8 +6,8 @@ void lqrCalc(struct sVars &);
void TVC(struct sVars &);
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);
float derivative(float x2, float x1, float dt);
float integral(float x2, float x1, float dt);
// Any parameters that are constants should be declared here instead of buried
// in code
@ -55,12 +55,12 @@ void sim(struct sVars &Vars) {
}
void burnStartTimeCalc(struct sVars &Vars) {
double v = Vars.vz;
double h = 0;
float v = Vars.vz;
float h = 0;
double a, j, m, thrust;
float a, j, m, thrust;
for (double i = 0.148; i < 3.450; i = i + dt) {
for (float i = 0.148; i < 3.450; i = i + dt) {
m = Vars.m0 - i * Vars.mdot;
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.vb = v; // terminal velocity
double burnStartTime = Vars.vb / -g;
float 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
float 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) &
@ -130,23 +130,23 @@ void lqrCalc(struct sVars &Vars) {
/*
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;
float n = sqrt(398600 / pow(6678, 3));
float k1 = (Vars.I22 - Vars.I33) / Vars.I11;
float k2 = (Vars.I11 - Vars.I33) / Vars.I22;
float k3 = (Vars.I22 - Vars.I11) / Vars.I33;
double R11 = pow(10, 2);
double R22 = pow(10, 2);
double R33 = pow(10, 2);
float R11 = pow(10, 2);
float R22 = pow(10, 2);
float 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;
float F11 = -2 * pow(n, 2) * 4 * k1;
float F22 = -2 * pow(n, 2) * 3 * k2;
float F33 = -2 * pow(n, 2) * k3;
double G31 = n * (1 - k1);
double G13 = n * (k3 - 1);
float G31 = n * (1 - k1);
float G13 = n * (k3 - 1);
double d = 0.5;
float 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.
@ -154,27 +154,27 @@ void lqrCalc(struct sVars &Vars) {
https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
*/
/*
double gain = 0.25;
float gain = 0.25;
double K11 =
float 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 =
float K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22);
float 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 =
float 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 =
float 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));
@ -186,26 +186,26 @@ void lqrCalc(struct sVars &Vars) {
*/
// 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;
float K11 = 39.54316;
float K12 = 0.00000;
float K13 = -0.00000;
float K14 = 39.55769;
float K15 = 0.00000;
float K16 = 0.00000;
float K21 = 0.00000;
float K22 = 39.54316;
float K23 = 0.00000;
float K24 = 0.00000;
float K25 = 39.55769;
float K26 = 0.00000;
float K31 = 0.00000;
float K32 = 0.00000;
float K33 = 39.54316;
float K34 = 0.00000;
float K35 = 0.00000;
float K36 = 39.54394;
double gain =
float gain =
0.25 *
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;
}
double derivative(double x2, double x1, double dt) {
double dxdt = (x2 - x1) / (dt / 1000);
float derivative(float x2, float x1, float dt) {
float dxdt = (x2 - x1) / (dt / 1000);
return dxdt;
}
double integral(double x, double y, double dt) {
double integ = (x * dt / 1000) + y;
float integral(float x, float y, float dt) {
float integ = (x * dt / 1000) + y;
return integ;
}