mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
Merge branch '5-relocate-initial-conditions' into 'main'
Resolve "Relocate Initial Conditions" Closes #5 See merge request lander-team/lander-cpp!1
This commit is contained in:
commit
78cd59dd11
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 39 KiB After Width: | Height: | Size: 35 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 33 KiB After Width: | Height: | Size: 33 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 31 KiB After Width: | Height: | Size: 30 KiB |
File diff suppressed because it is too large
Load Diff
@ -3,33 +3,27 @@
|
||||
#ifndef SVARS_H
|
||||
#define SVARS_H
|
||||
|
||||
struct sVars {
|
||||
struct Vehicle {
|
||||
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 ax, ay, az;
|
||||
|
||||
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 mass, massInitial, massPropellant, massBurnout, mdot;
|
||||
double vehicleHeight, vehicleRadius, momentArm;
|
||||
|
||||
double burntime;
|
||||
double burnVelocity;
|
||||
double thrust, thrust_prev, burnElapsed, burnStart;
|
||||
double thrust, burnElapsed, burnStart;
|
||||
double LQRx, LQRy, Fx, Fy, Fz;
|
||||
double momentX, momentY, momentZ;
|
||||
|
||||
double I11, I22, I33;
|
||||
double I11prev, I22prev, I33prev;
|
||||
double I11dot, I22dot, I33dot;
|
||||
|
||||
int maxServo;
|
||||
@ -39,4 +33,4 @@ struct sVars {
|
||||
int stepSize;
|
||||
};
|
||||
|
||||
#endif
|
||||
#endif
|
||||
390
include/sim.h
390
include/sim.h
@ -1,11 +1,11 @@
|
||||
#include "sVars.h"
|
||||
#include "Vehicle.h"
|
||||
|
||||
void burnStartTimeCalc(struct sVars &);
|
||||
void thrustSelection(struct sVars &, int t);
|
||||
void lqrCalc(struct sVars &);
|
||||
void TVC(struct sVars &);
|
||||
void vehicleDynamics(struct sVars &, int t);
|
||||
void write2CSV(struct sVars &, std::fstream &outfile, int t);
|
||||
void burnStartTimeCalc(struct Vehicle &);
|
||||
void thrustSelection(struct Vehicle &, int t);
|
||||
void lqrCalc(struct Vehicle &);
|
||||
void TVC(struct Vehicle &);
|
||||
void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
|
||||
void write2CSV(struct Vehicle &, std::fstream &outfile, int t);
|
||||
double derivative(double x2, double x1, double dt);
|
||||
double integral(double x2, double x1, double dt);
|
||||
|
||||
@ -14,14 +14,14 @@ double integral(double x2, double x1, double dt);
|
||||
double const dt = 0.001;
|
||||
double const g = -9.81;
|
||||
|
||||
void sim(struct sVars &Vars) {
|
||||
void sim(struct Vehicle &State, struct Vehicle &PrevState) {
|
||||
|
||||
// defining a few random values here cause I'm lazy
|
||||
Vars.burnElapsed = 2000;
|
||||
Vars.mass = Vars.massInitial;
|
||||
Vars.thrust_prev = 0;
|
||||
State.burnElapsed = 2000;
|
||||
State.mass = State.massInitial;
|
||||
PrevState.thrust = 0;
|
||||
|
||||
burnStartTimeCalc(Vars);
|
||||
burnStartTimeCalc(State);
|
||||
|
||||
// Deleting any previous output file
|
||||
if (remove("simOut.csv") != 0)
|
||||
@ -43,26 +43,26 @@ void sim(struct sVars &Vars) {
|
||||
<< std::endl;
|
||||
|
||||
// Start Sim
|
||||
for (int t = 0; t < Vars.simTime; t++) {
|
||||
thrustSelection(Vars, t);
|
||||
lqrCalc(Vars);
|
||||
TVC(Vars);
|
||||
vehicleDynamics(Vars, t);
|
||||
write2CSV(Vars, outfile, t);
|
||||
for (int t = 0; t < State.simTime; t++) {
|
||||
thrustSelection(State, t);
|
||||
lqrCalc(State);
|
||||
TVC(State);
|
||||
vehicleDynamics(State, PrevState, t);
|
||||
write2CSV(State, outfile, t);
|
||||
}
|
||||
|
||||
outfile.close();
|
||||
}
|
||||
|
||||
void burnStartTimeCalc(struct sVars &Vars) {
|
||||
double velocity = Vars.vz;
|
||||
void burnStartTimeCalc(struct Vehicle &State) {
|
||||
double velocity = State.vz;
|
||||
double h = 0;
|
||||
|
||||
double a, j, mass, thrust;
|
||||
|
||||
// Piecewise functions for F15 thrust curve
|
||||
for (double i = 0.148; i < 3.450; i = i + dt) {
|
||||
mass = Vars.massInitial - i * Vars.mdot;
|
||||
mass = State.massInitial - i * State.mdot;
|
||||
|
||||
if ((i > 0.147) && (i < 0.420))
|
||||
thrust = 65.165 * i - 2.3921;
|
||||
@ -78,51 +78,53 @@ void burnStartTimeCalc(struct sVars &Vars) {
|
||||
h = velocity * dt + h;
|
||||
}
|
||||
|
||||
Vars.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
|
||||
Vars.burnVelocity = velocity; // terminal velocity
|
||||
State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
|
||||
State.burnVelocity = velocity; // terminal velocity
|
||||
|
||||
double burnStartTime = Vars.burnVelocity / -g;
|
||||
Vars.simTime = (Vars.burntime + burnStartTime) * 1000;
|
||||
double burnStartTime = State.burnVelocity / -g;
|
||||
State.simTime = (State.burntime + burnStartTime) * 1000;
|
||||
}
|
||||
|
||||
void thrustSelection(struct sVars &Vars, int t) {
|
||||
void thrustSelection(struct Vehicle &State, int t) {
|
||||
|
||||
if (Vars.burnElapsed != 2000) {
|
||||
if (State.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.mass = Vars.massInitial - (Vars.mdot * Vars.burnElapsed);
|
||||
State.burnElapsed = (t - State.burnStart) / 1000;
|
||||
State.mass = State.massInitial - (State.mdot * State.burnElapsed);
|
||||
}
|
||||
|
||||
else if (abs(Vars.burnVelocity - Vars.vz) < .001) {
|
||||
else if (abs(State.burnVelocity - State.vz) < .001) {
|
||||
// Start burn
|
||||
Vars.burnStart = t;
|
||||
Vars.burnElapsed = 0;
|
||||
State.burnStart = t;
|
||||
State.burnElapsed = 0;
|
||||
}
|
||||
|
||||
else
|
||||
Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn
|
||||
State.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 ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420))
|
||||
State.thrust = 65.165 * State.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 ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
|
||||
State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
|
||||
11.609 * pow(State.burnElapsed, 5) +
|
||||
60.739 * pow(State.burnElapsed, 4) -
|
||||
162.99 * pow(State.burnElapsed, 3) +
|
||||
235.6 * pow(State.burnElapsed, 2) -
|
||||
174.43 * State.burnElapsed + 67.17;
|
||||
|
||||
else if ((Vars.burnElapsed > 3.382) && (Vars.burnElapsed < 3.46))
|
||||
Vars.thrust = -195.78 * Vars.burnElapsed + 675.11;
|
||||
else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
|
||||
State.thrust = -195.78 * State.burnElapsed + 675.11;
|
||||
}
|
||||
|
||||
void lqrCalc(struct sVars &Vars) {
|
||||
void lqrCalc(struct Vehicle &State) {
|
||||
|
||||
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);
|
||||
State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
|
||||
pow(State.vehicleRadius, 2) / 4);
|
||||
State.I22 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
|
||||
pow(State.vehicleRadius, 2) / 4);
|
||||
State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
|
||||
|
||||
// Paste in Values from gainCalc.m
|
||||
double K11 = 39.54316;
|
||||
@ -148,222 +150,224 @@ void lqrCalc(struct sVars &Vars) {
|
||||
double gain = 0.25 * pow(10, -4);
|
||||
|
||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
|
||||
Vars.LQRx =
|
||||
State.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 =
|
||||
((K12 * State.pitch) / 2 + K15 * State.pitchdot + (K13 * State.roll) / 2 +
|
||||
K16 * State.rolldot + (K11 * State.yaw) / 2 + K14 * State.yawdot) /
|
||||
-State.momentArm;
|
||||
State.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;
|
||||
((K22 * State.pitch) / 2 + K25 * State.pitchdot + (K23 * State.roll) / 2 +
|
||||
K26 * State.rolldot + (K21 * State.yaw) / 2 + K24 * State.yawdot) /
|
||||
-State.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;
|
||||
if (State.LQRx > State.thrust)
|
||||
State.LQRx = State.thrust;
|
||||
else if (State.LQRx < -1 * State.thrust)
|
||||
State.LQRx = -1 * State.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;
|
||||
if (State.LQRy > State.thrust)
|
||||
State.LQRy = State.thrust;
|
||||
else if (State.LQRy < -1 * State.thrust)
|
||||
State.LQRy = -1 * State.thrust;
|
||||
}
|
||||
|
||||
void TVC(struct sVars &Vars) {
|
||||
if (Vars.thrust < 1) {
|
||||
void TVC(struct Vehicle &State) {
|
||||
if (State.thrust < 1) {
|
||||
// Define forces and moments for t = 0
|
||||
Vars.Fx = 0;
|
||||
Vars.Fy = 0;
|
||||
Vars.Fz = g * Vars.massInitial;
|
||||
State.Fx = 0;
|
||||
State.Fy = 0;
|
||||
State.Fz = g * State.massInitial;
|
||||
|
||||
Vars.momentX = 0;
|
||||
Vars.momentY = 0;
|
||||
Vars.momentZ = 0;
|
||||
State.momentX = 0;
|
||||
State.momentY = 0;
|
||||
State.momentZ = 0;
|
||||
} else {
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
Vars.xServoDegs = (180 / M_PI) * asin(Vars.LQRx / Vars.thrust);
|
||||
State.xServoDegs = (180 / M_PI) * asin(State.LQRx / State.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;
|
||||
if (State.xServoDegs > State.maxServo)
|
||||
State.xServoDegs = State.maxServo;
|
||||
else if (State.xServoDegs < -1 * State.maxServo)
|
||||
State.xServoDegs = -1 * State.maxServo;
|
||||
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
Vars.yServoDegs = (180 / M_PI) * asin(Vars.LQRy / Vars.thrust);
|
||||
State.yServoDegs = (180 / M_PI) * asin(State.LQRy / State.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;
|
||||
if (State.yServoDegs > State.maxServo)
|
||||
State.yServoDegs = State.maxServo;
|
||||
else if (State.yServoDegs < -1 * State.maxServo)
|
||||
State.yServoDegs = -1 * State.maxServo;
|
||||
|
||||
// Vector math to aqcuire thrust vector components
|
||||
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.mass * g);
|
||||
State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
|
||||
State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
|
||||
State.Fz =
|
||||
sqrt(pow(State.thrust, 2) - (pow(State.Fx, 2) + pow(State.Fy, 2))) +
|
||||
(State.mass * g);
|
||||
|
||||
// Calculate moment created by Fx and Fy
|
||||
Vars.momentX = Vars.Fx * Vars.momentArm;
|
||||
Vars.momentY = Vars.Fy * Vars.momentArm;
|
||||
Vars.momentZ = 0;
|
||||
State.momentX = State.Fx * State.momentArm;
|
||||
State.momentY = State.Fy * State.momentArm;
|
||||
State.momentZ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void vehicleDynamics(struct sVars &Vars, int t) {
|
||||
void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
|
||||
// Idot
|
||||
if (t < 1) {
|
||||
Vars.I11dot = 0;
|
||||
Vars.I22dot = 0;
|
||||
Vars.I33dot = 0;
|
||||
State.I11dot = 0;
|
||||
State.I22dot = 0;
|
||||
State.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);
|
||||
State.I11dot = derivative(State.I11, PrevState.I11, State.stepSize);
|
||||
State.I22dot = derivative(State.I22, PrevState.I22, State.stepSize);
|
||||
State.I33dot = derivative(State.I33, PrevState.I33, State.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;
|
||||
State.yawddot = (State.momentX - State.I11dot * State.yawdot +
|
||||
State.I22 * State.pitchdot * State.rolldot -
|
||||
State.I33 * State.pitchdot * State.rolldot) /
|
||||
State.I11;
|
||||
State.pitchddot = (State.momentY - State.I22dot * State.pitchdot -
|
||||
State.I11 * State.rolldot * State.yawdot +
|
||||
State.I33 * State.rolldot * State.yawdot) /
|
||||
State.I22;
|
||||
State.rollddot = (State.momentZ - State.I33dot * State.rolldot +
|
||||
State.I11 * State.pitchdot * State.yawdot -
|
||||
State.I22 * State.pitchdot * State.yawdot) /
|
||||
State.I33;
|
||||
|
||||
if (t < 1) {
|
||||
Vars.x = 0;
|
||||
Vars.y = 0;
|
||||
State.x = 0;
|
||||
State.y = 0;
|
||||
|
||||
Vars.ax = 0;
|
||||
Vars.ay = 0;
|
||||
Vars.az = Vars.Fz / Vars.massInitial;
|
||||
State.ax = 0;
|
||||
State.ay = 0;
|
||||
State.az = State.Fz / State.massInitial;
|
||||
}
|
||||
|
||||
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);
|
||||
State.yawdot = integral(State.yawddot, PrevState.yawdot, State.stepSize);
|
||||
State.pitchdot =
|
||||
integral(State.pitchddot, PrevState.pitchdot, State.stepSize);
|
||||
State.rolldot = integral(State.rollddot, PrevState.rolldot, State.stepSize);
|
||||
|
||||
// ax ay az
|
||||
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);
|
||||
State.ax = (State.Fx / State.mass) +
|
||||
(State.pitchdot * State.vz - State.rolldot * State.vy);
|
||||
State.ay = (State.Fy / State.mass) +
|
||||
(State.rolldot * State.vx - State.vz * State.yawdot);
|
||||
State.az = (State.Fz / State.mass) +
|
||||
(State.vy * State.yawdot - State.pitchdot * State.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);
|
||||
State.vx = integral(State.ax, PrevState.vx, State.stepSize);
|
||||
State.vy = integral(State.ay, PrevState.vy, State.stepSize);
|
||||
State.vz = integral(State.az, PrevState.vz, State.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);
|
||||
State.x = integral(State.vx, PrevState.x, State.stepSize);
|
||||
State.y = integral(State.vy, PrevState.y, State.stepSize);
|
||||
State.z = integral(State.vz, PrevState.z, State.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);
|
||||
State.phidot = State.yawdot + (State.pitchdot * sin(State.yaw) +
|
||||
State.rolldot * cos(State.yaw)) *
|
||||
(sin(State.pitch) / cos(State.pitch));
|
||||
State.thetadot =
|
||||
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.pitch);
|
||||
State.psidot =
|
||||
(State.pitchdot * sin(State.yaw) + State.rolldot * cos(State.yaw)) /
|
||||
cos(State.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);
|
||||
State.yaw = integral(State.phidot, PrevState.yaw, State.stepSize);
|
||||
State.pitch = integral(State.thetadot, PrevState.pitch, State.stepSize);
|
||||
State.roll = integral(State.psidot, PrevState.roll, State.stepSize);
|
||||
}
|
||||
|
||||
// Set "prev" values for next timestep
|
||||
Vars.I11prev = Vars.I11;
|
||||
Vars.I22prev = Vars.I22;
|
||||
Vars.I33prev = Vars.I33;
|
||||
PrevState.I11 = State.I11;
|
||||
PrevState.I22 = State.I22;
|
||||
PrevState.I33 = State.I33;
|
||||
|
||||
Vars.yawPrev = Vars.yaw;
|
||||
Vars.pitchPrev = Vars.pitch;
|
||||
Vars.rollPrev = Vars.roll;
|
||||
PrevState.yaw = State.yaw;
|
||||
PrevState.pitch = State.pitch;
|
||||
PrevState.roll = State.roll;
|
||||
|
||||
Vars.yawdotPrev = Vars.yawdot;
|
||||
Vars.pitchdotPrev = Vars.pitchdot;
|
||||
Vars.rolldotPrev = Vars.rolldot;
|
||||
PrevState.yawdot = State.yawdot;
|
||||
PrevState.pitchdot = State.pitchdot;
|
||||
PrevState.rolldot = State.rolldot;
|
||||
|
||||
Vars.yawddotPrev = Vars.yawddot;
|
||||
Vars.pitchddotPrev = Vars.pitchddot;
|
||||
Vars.rollddotPrev = Vars.rollddot;
|
||||
PrevState.yawddot = State.yawddot;
|
||||
PrevState.pitchddot = State.pitchddot;
|
||||
PrevState.rollddot = State.rollddot;
|
||||
|
||||
Vars.axPrev = Vars.ax;
|
||||
Vars.ayPrev = Vars.ay;
|
||||
Vars.azPrev = Vars.az;
|
||||
PrevState.ax = State.ax;
|
||||
PrevState.ay = State.ay;
|
||||
PrevState.az = State.az;
|
||||
|
||||
Vars.vxPrev = Vars.vx;
|
||||
Vars.vyPrev = Vars.vy;
|
||||
Vars.vzPrev = Vars.vz;
|
||||
PrevState.vx = State.vx;
|
||||
PrevState.vy = State.vy;
|
||||
PrevState.vz = State.vz;
|
||||
|
||||
Vars.xPrev = Vars.x;
|
||||
Vars.yPrev = Vars.y;
|
||||
Vars.zPrev = Vars.z;
|
||||
PrevState.x = State.x;
|
||||
PrevState.y = State.y;
|
||||
PrevState.z = State.z;
|
||||
}
|
||||
|
||||
void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
|
||||
void write2CSV(struct Vehicle &State, std::fstream &outfile, int t) {
|
||||
// writing to output file
|
||||
outfile << t << ", ";
|
||||
|
||||
outfile << Vars.x << ", ";
|
||||
outfile << Vars.y << ", ";
|
||||
outfile << Vars.z << ", ";
|
||||
outfile << State.x << ", ";
|
||||
outfile << State.y << ", ";
|
||||
outfile << State.z << ", ";
|
||||
|
||||
outfile << Vars.vx << ", ";
|
||||
outfile << Vars.vy << ", ";
|
||||
outfile << Vars.vz << ", ";
|
||||
outfile << State.vx << ", ";
|
||||
outfile << State.vy << ", ";
|
||||
outfile << State.vz << ", ";
|
||||
|
||||
outfile << Vars.ax << ", ";
|
||||
outfile << Vars.ay << ", ";
|
||||
outfile << Vars.az << ", ";
|
||||
outfile << State.ax << ", ";
|
||||
outfile << State.ay << ", ";
|
||||
outfile << State.az << ", ";
|
||||
|
||||
outfile << Vars.yaw * 180 / M_PI << ", ";
|
||||
outfile << Vars.pitch * 180 / M_PI << ", ";
|
||||
outfile << Vars.roll * 180 / M_PI << ", ";
|
||||
outfile << State.yaw * 180 / M_PI << ", ";
|
||||
outfile << State.pitch * 180 / M_PI << ", ";
|
||||
outfile << State.roll * 180 / M_PI << ", ";
|
||||
|
||||
outfile << Vars.yawdot * 180 / M_PI << ", ";
|
||||
outfile << Vars.pitchdot * 180 / M_PI << ", ";
|
||||
outfile << Vars.rolldot * 180 / M_PI << ", ";
|
||||
outfile << State.yawdot * 180 / M_PI << ", ";
|
||||
outfile << State.pitchdot * 180 / M_PI << ", ";
|
||||
outfile << State.rolldot * 180 / M_PI << ", ";
|
||||
|
||||
outfile << Vars.yawddot * 180 / M_PI << ", ";
|
||||
outfile << Vars.pitchddot * 180 / M_PI << ", ";
|
||||
outfile << Vars.rollddot * 180 / M_PI << ", ";
|
||||
outfile << State.yawddot * 180 / M_PI << ", ";
|
||||
outfile << State.pitchddot * 180 / M_PI << ", ";
|
||||
outfile << State.rollddot * 180 / M_PI << ", ";
|
||||
|
||||
outfile << Vars.I11 << ", ";
|
||||
outfile << Vars.I22 << ", ";
|
||||
outfile << Vars.I33 << ", ";
|
||||
outfile << State.I11 << ", ";
|
||||
outfile << State.I22 << ", ";
|
||||
outfile << State.I33 << ", ";
|
||||
|
||||
outfile << Vars.I11dot << ", ";
|
||||
outfile << Vars.I22dot << ", ";
|
||||
outfile << Vars.I33dot << ", ";
|
||||
outfile << State.I11dot << ", ";
|
||||
outfile << State.I22dot << ", ";
|
||||
outfile << State.I33dot << ", ";
|
||||
|
||||
outfile << Vars.xServoDegs << ", ";
|
||||
outfile << Vars.yServoDegs << ", ";
|
||||
outfile << State.xServoDegs << ", ";
|
||||
outfile << State.yServoDegs << ", ";
|
||||
|
||||
outfile << Vars.mass << ", ";
|
||||
outfile << Vars.thrust << ", ";
|
||||
outfile << Vars.burnElapsed << ", ";
|
||||
outfile << Vars.Fz << ", ";
|
||||
outfile << State.mass << ", ";
|
||||
outfile << State.thrust << ", ";
|
||||
outfile << State.burnElapsed << ", ";
|
||||
outfile << State.Fz << ", ";
|
||||
|
||||
outfile << Vars.LQRx << ", ";
|
||||
outfile << Vars.LQRy << std::endl;
|
||||
outfile << State.LQRx << ", ";
|
||||
outfile << State.LQRy << std::endl;
|
||||
}
|
||||
|
||||
double derivative(double x2, double x1, double dt) {
|
||||
|
||||
47
src/main.cpp
47
src/main.cpp
@ -5,48 +5,49 @@
|
||||
#include <iostream>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "sVars.h"
|
||||
#include "Vehicle.h"
|
||||
#include "sim.h"
|
||||
|
||||
void sim(struct sVars &);
|
||||
void sim(struct Vehicle &);
|
||||
|
||||
int main() {
|
||||
sVars Vars;
|
||||
Vehicle State;
|
||||
Vehicle PrevState;
|
||||
|
||||
// Initial Velocity
|
||||
Vars.vx = 0; // [m/s]
|
||||
Vars.vy = 0; // [m/s]
|
||||
Vars.vz = 0; // [m/s]
|
||||
State.vx = 0; // [m/s]
|
||||
State.vy = 0; // [m/s]
|
||||
State.vz = 0; // [m/s]
|
||||
|
||||
// Initial YPR
|
||||
Vars.yaw = 0 * M_PI / 180; // [rad]
|
||||
Vars.pitch = 0 * M_PI / 180; // [rad]
|
||||
Vars.roll = 0 * M_PI / 180; // [rad]
|
||||
State.yaw = 0 * M_PI / 180; // [rad]
|
||||
State.pitch = 0 * M_PI / 180; // [rad]
|
||||
State.roll = 0 * M_PI / 180; // [rad]
|
||||
|
||||
// Initial YPRdot
|
||||
Vars.yawdot = 0 * M_PI / 180; // [rad/s]
|
||||
Vars.pitchdot = 0 * M_PI / 180; // [rad/s]
|
||||
Vars.rolldot = 0 * M_PI / 180; // [rad/s]
|
||||
State.yawdot = 0 * M_PI / 180; // [rad/s]
|
||||
State.pitchdot = 0 * M_PI / 180; // [rad/s]
|
||||
State.rolldot = 0 * M_PI / 180; // [rad/s]
|
||||
|
||||
// Servo Limitation
|
||||
Vars.maxServo = 15; // [degs]
|
||||
State.maxServo = 15; // [degs]
|
||||
|
||||
// Vehicle Properties
|
||||
Vars.massInitial = 1.2; // [kg]
|
||||
Vars.vehicleHeight = 0.5318; // [m]
|
||||
Vars.vehicleRadius = 0.05105; // [m]
|
||||
Vars.momentArm = 0.145; // [m]
|
||||
State.massInitial = 1.2; // [kg]
|
||||
State.vehicleHeight = 0.5318; // [m]
|
||||
State.vehicleRadius = 0.05105; // [m]
|
||||
State.momentArm = 0.145; // [m]
|
||||
|
||||
// Sim Step Size
|
||||
Vars.stepSize = 1; // [ms]
|
||||
State.stepSize = 1; // [ms]
|
||||
|
||||
// Other Properties
|
||||
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]
|
||||
State.massPropellant = 0.06; // [kg]
|
||||
State.massBurnout = State.massInitial - State.massPropellant; // [kg]
|
||||
State.burntime = 3.45 - 0.148; // [s]
|
||||
State.mdot = State.massPropellant / State.burntime; // [kg/s]
|
||||
|
||||
sim(Vars);
|
||||
sim(State, PrevState);
|
||||
|
||||
std::cout << "Finished";
|
||||
std::cin.get();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user