Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 23:26:43 +00:00
Code Releases Activity

lots of code cleanup and variable renaming

Browse Source
This commit is contained in:
Anson Biggs 2021-09-13 14:22:57 -07:00
parent 804bc637b8
commit 952ffaac4e
4 changed files with 52 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/extensions.json vendored
View File

@ -1,5 +1,6 @@
{ {
"recommendations": [ "recommendations": [
"ms-vscode.cpptools" "ms-vscode.cpptools",
"wayou.vscode-todo-highlight"
] ]
} }

6
include/sVars.h
View File

@ -19,11 +19,11 @@ struct sVars {
double yawddot, pitchddot, rollddot; double yawddot, pitchddot, rollddot;
double yawddotPrev, pitchddotPrev, rollddotPrev; double yawddotPrev, pitchddotPrev, rollddotPrev;
double m, m0, mp, mb, mdot; double mass, massInitial, massPropellant, massBurnout, mdot;
double vehicleHeight, vehicleRadius, momentArm; double vehicleHeight, vehicleRadius, momentArm;
double tb; double burntime;
double vb; double burnVelocity;
double thrust, thrust_prev, burnElapsed, burnStart; double thrust, thrust_prev, burnElapsed, burnStart;
double LQRx, LQRy, Fx, Fy, Fz; double LQRx, LQRy, Fx, Fy, Fz;
double momentX, momentY, momentZ; double momentX, momentY, momentZ;

78
include/sim.h
View File

@ -18,7 +18,7 @@ void sim(struct sVars &Vars) {
// defining a few random values here cause I'm lazy // defining a few random values here cause I'm lazy
Vars.burnElapsed = 2000; Vars.burnElapsed = 2000;
Vars.m = Vars.m0; Vars.mass = Vars.massInitial;
Vars.thrust_prev = 0; Vars.thrust_prev = 0;
burnStartTimeCalc(Vars); burnStartTimeCalc(Vars);
@ -48,20 +48,22 @@ void sim(struct sVars &Vars) {
lqrCalc(Vars); lqrCalc(Vars);
TVC(Vars); TVC(Vars);
vehicleDynamics(Vars, t); vehicleDynamics(Vars, t);
write2CSV(Vars, outfile, t); write2CSV(Vars, outfile,
t); // TODO: Need to append to memory instead of writing each step
} }
outfile.close(); outfile.close();
} }
void burnStartTimeCalc(struct sVars &Vars) { void burnStartTimeCalc(struct sVars &Vars) {
double v = Vars.vz; double velocity = Vars.vz;
double h = 0; double h = 0;
double a, j, m, thrust; double a, j, mass, thrust;
// Piecewise functions for F15 thrust curve
for (double 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; mass = Vars.massInitial - i * Vars.mdot;
if ((i > 0.147) && (i < 0.420)) if ((i > 0.147) && (i < 0.420))
thrust = 65.165 * i - 2.3921; thrust = 65.165 * i - 2.3921;
@ -73,29 +75,30 @@ void burnStartTimeCalc(struct sVars &Vars) {
else if ((i > 3.382) && (i < 3.46)) else if ((i > 3.382) && (i < 3.46))
thrust = -195.78 * i + 675.11; thrust = -195.78 * i + 675.11;
v = (((thrust / m) + g) * dt) + v; velocity = (((thrust / mass) + g) * dt) + velocity;
h = v * dt + h; h = velocity * dt + h;
} }
Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height Vars.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
Vars.vb = v; // terminal velocity Vars.burnVelocity = velocity; // terminal velocity
double burnStartTime = Vars.vb / -g; double burnStartTime = Vars.burnVelocity / -g;
Vars.simTime = (Vars.tb + burnStartTime) * 1000; Vars.simTime = (Vars.burntime + burnStartTime) * 1000;
} }
void thrustSelection(struct sVars &Vars, int t) { void thrustSelection(struct sVars &Vars, int t) {
// TODO: Determine a real tolerance
double 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.burnVelocity < (1 + tol) * Vars.vz * -1) &
(Vars.vb > (1 - tol) * Vars.vz * -1); (Vars.burnVelocity > (1 - tol) * Vars.vz * -1);
if (Vars.burnElapsed != 2000) { if (Vars.burnElapsed != 2000) {
// determine where in the thrust curve we're at based on elapsed burn time // determine where in the thrust curve we're at based on elapsed burn time
// as well as current mass // as well as current mass
Vars.burnElapsed = (t - Vars.burnStart) / 1000; Vars.burnElapsed = (t - Vars.burnStart) / 1000;
Vars.m = Vars.m0 - (Vars.mdot * Vars.burnElapsed); Vars.mass = Vars.massInitial - (Vars.mdot * Vars.burnElapsed);
} }
else if (b_burnStart) { else if (b_burnStart) {
@ -104,7 +107,7 @@ void thrustSelection(struct sVars &Vars, int t) {
Vars.burnElapsed = 0; Vars.burnElapsed = 0;
} }
else else // TODO: Make this not arbitrary
Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn
if ((Vars.burnElapsed > 0.147) && (Vars.burnElapsed < 0.420)) if ((Vars.burnElapsed > 0.147) && (Vars.burnElapsed < 0.420))
@ -122,11 +125,11 @@ void thrustSelection(struct sVars &Vars, int t) {
void lqrCalc(struct sVars &Vars) { void lqrCalc(struct sVars &Vars) {
Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) + Vars.I11 = Vars.mass * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
pow(Vars.vehicleRadius, 2) / 4); pow(Vars.vehicleRadius, 2) / 4);
Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) + Vars.I22 = Vars.mass * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
pow(Vars.vehicleRadius, 2) / 4); pow(Vars.vehicleRadius, 2) / 4);
Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2); Vars.I33 = Vars.mass * 0.5 * pow(Vars.vehicleRadius, 2);
// Paste in Values from gainCalc.m // Paste in Values from gainCalc.m
double K11 = 39.54316; double K11 = 39.54316;
@ -148,9 +151,8 @@ void lqrCalc(struct sVars &Vars) {
double K35 = 0.00000; double K35 = 0.00000;
double K36 = 39.54394; double K36 = 39.54394;
double gain = // changing gain exponent drastically changes results of LQR
0.25 * double gain = 0.25 * pow(10, -4);
pow(10, -4); // changing exponenet drastically changes results of LQR
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
Vars.LQRx = Vars.LQRx =
@ -182,14 +184,12 @@ void TVC(struct sVars &Vars) {
// Define forces and moments for t = 0 // Define forces and moments for t = 0
Vars.Fx = 0; Vars.Fx = 0;
Vars.Fy = 0; Vars.Fy = 0;
Vars.Fz = g * Vars.m0; Vars.Fz = g * Vars.massInitial;
Vars.momentX = 0; Vars.momentX = 0;
Vars.momentY = 0; Vars.momentY = 0;
Vars.momentZ = 0; Vars.momentZ = 0;
} } else {
else {
// Convert servo position to degrees for comparison to max allowable // Convert servo position to degrees for comparison to max allowable
Vars.xServoDegs = (180 / M_PI) * asin(Vars.LQRx / Vars.thrust); Vars.xServoDegs = (180 / M_PI) * asin(Vars.LQRx / Vars.thrust);
@ -212,7 +212,7 @@ void TVC(struct sVars &Vars) {
Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (M_PI / 180)); Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (M_PI / 180));
Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (M_PI / 180)); Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (M_PI / 180));
Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) + Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) +
(Vars.m * g); (Vars.mass * g);
// Calculate moment created by Fx and Fy // Calculate moment created by Fx and Fy
Vars.momentX = Vars.Fx * Vars.momentArm; Vars.momentX = Vars.Fx * Vars.momentArm;
@ -223,7 +223,7 @@ void TVC(struct sVars &Vars) {
void vehicleDynamics(struct sVars &Vars, int t) { void vehicleDynamics(struct sVars &Vars, int t) {
// Idot // Idot
if (t < 1) { if (t < 1) { // TODO: Initial conditions should be set at sim start
Vars.I11dot = 0; Vars.I11dot = 0;
Vars.I22dot = 0; Vars.I22dot = 0;
Vars.I33dot = 0; Vars.I33dot = 0;
@ -253,7 +253,7 @@ void vehicleDynamics(struct sVars &Vars, int t) {
Vars.ax = 0; Vars.ax = 0;
Vars.ay = 0; Vars.ay = 0;
Vars.az = Vars.Fz / Vars.m0; Vars.az = Vars.Fz / Vars.massInitial;
} }
else { else {
@ -263,12 +263,12 @@ void vehicleDynamics(struct sVars &Vars, int t) {
Vars.rolldot = integral(Vars.rollddot, Vars.rolldotPrev, Vars.stepSize); Vars.rolldot = integral(Vars.rollddot, Vars.rolldotPrev, Vars.stepSize);
// ax ay az // ax ay az
Vars.ax = Vars.ax = (Vars.Fx / Vars.mass) +
(Vars.Fx / Vars.m) + (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy); (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy);
Vars.ay = Vars.ay = (Vars.Fy / Vars.mass) +
(Vars.Fy / Vars.m) + (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot); (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot);
Vars.az = Vars.az = (Vars.Fz / Vars.mass) +
(Vars.Fz / Vars.m) + (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx); (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx);
// vx vy vz in Body frame // vx vy vz in Body frame
Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize); Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize);
@ -295,6 +295,9 @@ void vehicleDynamics(struct sVars &Vars, int t) {
Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize); Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize);
} }
// TODO: Maybe we should just have a `Vars` for the last time step and at the
// end of each time step make a copy of it
// Set "prev" values for next timestep // Set "prev" values for next timestep
Vars.I11prev = Vars.I11; Vars.I11prev = Vars.I11;
Vars.I22prev = Vars.I22; Vars.I22prev = Vars.I22;
@ -364,7 +367,7 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
outfile << Vars.xServoDegs << ", "; outfile << Vars.xServoDegs << ", ";
outfile << Vars.yServoDegs << ", "; outfile << Vars.yServoDegs << ", ";
outfile << Vars.m << ", "; outfile << Vars.mass << ", ";
outfile << Vars.thrust << ", "; outfile << Vars.thrust << ", ";
outfile << Vars.burnElapsed << ", "; outfile << Vars.burnElapsed << ", ";
outfile << Vars.Fz << ", "; outfile << Vars.Fz << ", ";
@ -374,6 +377,7 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
} }
double derivative(double x2, double x1, double dt) { double derivative(double x2, double x1, double dt) {
// TODO: is 1000 arbitrary?
double dxdt = (x2 - x1) / (dt / 1000); double dxdt = (x2 - x1) / (dt / 1000);
return dxdt; return dxdt;
} }

11
src/main.cpp
View File

@ -30,19 +30,20 @@ int main() {
Vars.maxServo = 15; // [degs] Vars.maxServo = 15; // [degs]
// Vehicle Properties // Vehicle Properties
Vars.m0 = 1.2; // [kg] Vars.massInitial = 1.2; // [kg]
Vars.vehicleHeight = 0.5318; // [m] Vars.vehicleHeight = 0.5318; // [m]
Vars.vehicleRadius = 0.05105; // [m] Vars.vehicleRadius = 0.05105; // [m]
Vars.momentArm = 0.145; // [m] Vars.momentArm = 0.145; // [m]
// Sim Step Size // Sim Step Size
// TODO: use dt instead?
Vars.stepSize = 1; // [ms] Vars.stepSize = 1; // [ms]
// Other Properties // Other Properties
Vars.mp = 0.06; // [kg] Vars.massPropellant = 0.06; // [kg]
Vars.mb = Vars.m0 - Vars.mp; // [kg] Vars.massBurnout = Vars.massInitial - Vars.massPropellant; // [kg]
Vars.tb = 3.45 - 0.148; // [s] Vars.burntime = 3.45 - 0.148; // [s]
Vars.mdot = Vars.mp / Vars.tb; // [kg/s] Vars.mdot = Vars.massPropellant / Vars.burntime; // [kg/s]
sim(Vars); sim(Vars);