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

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": [
"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 yawddotPrev, pitchddotPrev, rollddotPrev;
double m, m0, mp, mb, mdot;
double mass, massInitial, massPropellant, massBurnout, mdot;
double vehicleHeight, vehicleRadius, momentArm;
double tb;
double vb;
double burntime;
double burnVelocity;
double thrust, thrust_prev, burnElapsed, burnStart;
double LQRx, LQRy, Fx, Fy, Fz;
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
Vars.burnElapsed = 2000;
Vars.m = Vars.m0;
Vars.mass = Vars.massInitial;
Vars.thrust_prev = 0;
burnStartTimeCalc(Vars);
@ -48,20 +48,22 @@ void sim(struct sVars &Vars) {
lqrCalc(Vars);
TVC(Vars);
vehicleDynamics(Vars, t);
write2CSV(Vars, outfile, t);
write2CSV(Vars, outfile,
t); // TODO: Need to append to memory instead of writing each step
}
outfile.close();
}
void burnStartTimeCalc(struct sVars &Vars) {
double v = Vars.vz;
double velocity = Vars.vz;
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) {
m = Vars.m0 - i * Vars.mdot;
mass = Vars.massInitial - i * Vars.mdot;
if ((i > 0.147) && (i < 0.420))
thrust = 65.165 * i - 2.3921;
@ -73,29 +75,30 @@ void burnStartTimeCalc(struct sVars &Vars) {
else if ((i > 3.382) && (i < 3.46))
thrust = -195.78 * i + 675.11;
v = (((thrust / m) + g) * dt) + v;
h = v * dt + h;
velocity = (((thrust / mass) + g) * dt) + velocity;
h = velocity * dt + h;
}
Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height
Vars.vb = v; // terminal velocity
Vars.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
Vars.burnVelocity = velocity; // terminal velocity
double burnStartTime = Vars.vb / -g;
Vars.simTime = (Vars.tb + burnStartTime) * 1000;
double burnStartTime = Vars.burnVelocity / -g;
Vars.simTime = (Vars.burntime + burnStartTime) * 1000;
}
void thrustSelection(struct sVars &Vars, int t) {
// TODO: Determine a real 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
bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) &
(Vars.vb > (1 - tol) * Vars.vz * -1);
bool b_burnStart = (Vars.burnVelocity < (1 + tol) * Vars.vz * -1) &
(Vars.burnVelocity > (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);
Vars.mass = Vars.massInitial - (Vars.mdot * Vars.burnElapsed);
}
else if (b_burnStart) {
@ -104,7 +107,7 @@ void thrustSelection(struct sVars &Vars, int t) {
Vars.burnElapsed = 0;
}
else
else // TODO: Make this not arbitrary
Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn
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) {
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.mass * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
pow(Vars.vehicleRadius, 2) / 4);
Vars.I22 = Vars.mass * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
pow(Vars.vehicleRadius, 2) / 4);
Vars.I33 = Vars.mass * 0.5 * pow(Vars.vehicleRadius, 2);
// Paste in Values from gainCalc.m
double K11 = 39.54316;
@ -148,9 +151,8 @@ void lqrCalc(struct sVars &Vars) {
double K35 = 0.00000;
double K36 = 39.54394;
double gain =
0.25 *
pow(10, -4); // changing exponenet drastically changes results of LQR
// changing gain exponent drastically changes results of LQR
double gain = 0.25 * pow(10, -4);
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
Vars.LQRx =
@ -182,14 +184,12 @@ void TVC(struct sVars &Vars) {
// Define forces and moments for t = 0
Vars.Fx = 0;
Vars.Fy = 0;
Vars.Fz = g * Vars.m0;
Vars.Fz = g * Vars.massInitial;
Vars.momentX = 0;
Vars.momentY = 0;
Vars.momentZ = 0;
}
else {
} else {
// Convert servo position to degrees for comparison to max allowable
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.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.m * g);
(Vars.mass * g);
// Calculate moment created by Fx and Fy
Vars.momentX = Vars.Fx * Vars.momentArm;
@ -223,7 +223,7 @@ void TVC(struct sVars &Vars) {
void vehicleDynamics(struct sVars &Vars, int t) {
// Idot
if (t < 1) {
if (t < 1) { // TODO: Initial conditions should be set at sim start
Vars.I11dot = 0;
Vars.I22dot = 0;
Vars.I33dot = 0;
@ -253,7 +253,7 @@ void vehicleDynamics(struct sVars &Vars, int t) {
Vars.ax = 0;
Vars.ay = 0;
Vars.az = Vars.Fz / Vars.m0;
Vars.az = Vars.Fz / Vars.massInitial;
}
else {
@ -263,12 +263,12 @@ void vehicleDynamics(struct sVars &Vars, int t) {
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);
Vars.ax = (Vars.Fx / Vars.mass) +
(Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy);
Vars.ay = (Vars.Fy / Vars.mass) +
(Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot);
Vars.az = (Vars.Fz / Vars.mass) +
(Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx);
// vx vy vz in Body frame
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);
}
// 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
Vars.I11prev = Vars.I11;
Vars.I22prev = Vars.I22;
@ -364,7 +367,7 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
outfile << Vars.xServoDegs << ", ";
outfile << Vars.yServoDegs << ", ";
outfile << Vars.m << ", ";
outfile << Vars.mass << ", ";
outfile << Vars.thrust << ", ";
outfile << Vars.burnElapsed << ", ";
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) {
// TODO: is 1000 arbitrary?
double dxdt = (x2 - x1) / (dt / 1000);
return dxdt;
}

11
src/main.cpp
View File

@ -30,19 +30,20 @@ int main() {
Vars.maxServo = 15; // [degs]
// Vehicle Properties
Vars.m0 = 1.2; // [kg]
Vars.massInitial = 1.2; // [kg]
Vars.vehicleHeight = 0.5318; // [m]
Vars.vehicleRadius = 0.05105; // [m]
Vars.momentArm = 0.145; // [m]
// Sim Step Size
// TODO: use dt instead?
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]
Vars.massPropellant = 0.06; // [kg]
Vars.massBurnout = Vars.massInitial - Vars.massPropellant; // [kg]
Vars.burntime = 3.45 - 0.148; // [s]
Vars.mdot = Vars.massPropellant / Vars.burntime; // [kg/s]
sim(Vars);