mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
258 lines
8.4 KiB
C++
258 lines
8.4 KiB
C++
#include "Vehicle.h"
|
|
#include "outVector.h"
|
|
|
|
#include <iostream>
|
|
|
|
void burnStartTimeCalc(struct Vehicle &);
|
|
void pidController(struct Vehicle &, struct Vehicle &);
|
|
void TVC(struct Vehicle &, struct Vehicle &);
|
|
void vehicleDynamics(struct Vehicle &, struct Vehicle &);
|
|
void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &);
|
|
double derivative(double current, double previous, double step);
|
|
double integral(double currentChange, double prevValue, double dt);
|
|
double limit(double value, double upr, double lwr);
|
|
|
|
// Any parameters that are constants should be declared here instead of
|
|
// buried in code
|
|
double const dt = 0.01;
|
|
double const g = -9.81;
|
|
|
|
void burnStartTimeCalc(Vehicle &State) {
|
|
double velocity = State.vz;
|
|
double h = 0;
|
|
|
|
double mass, thrust;
|
|
|
|
// Piecewise functions for F15 thrust curve
|
|
for (double i = 0.148; i < 3.450; i = i + dt) {
|
|
mass = State.massInitial - i * State.mdot;
|
|
|
|
if ((i > 0.147) && (i < 0.420))
|
|
thrust = 65.165 * i - 2.3921;
|
|
|
|
else if ((i > 0.419) && (i < 3.383))
|
|
thrust = 0.8932 * pow(i, 6) - 11.609 * pow(i, 5) + 60.739 * pow(i, 4) -
|
|
162.99 * pow(i, 3) + 235.6 * pow(i, 2) - 174.43 * i + 67.17;
|
|
|
|
else if ((i > 3.382) && (i < 3.46))
|
|
thrust = -195.78 * i + 675.11;
|
|
else
|
|
thrust = 0;
|
|
|
|
velocity = (((thrust / mass) + g) * dt) + velocity;
|
|
h = (((thrust / mass) + g) * dt) + h;
|
|
}
|
|
State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
|
|
State.z = 18.9;
|
|
State.burnVelocity = velocity; // terminal velocity
|
|
|
|
double burnStartTime = State.burnVelocity / -g;
|
|
State.simTime = (State.burntime + burnStartTime) * 1000;
|
|
}
|
|
|
|
void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
|
|
|
|
// Moment of Inertia
|
|
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);
|
|
|
|
// Idot
|
|
if (State.time < 0.1) {
|
|
State.I11dot = 0;
|
|
State.I22dot = 0;
|
|
State.I33dot = 0;
|
|
|
|
State.x = 0;
|
|
State.y = 0;
|
|
|
|
State.ax = 0;
|
|
State.ay = 0;
|
|
State.az = State.Fz / State.massInitial;
|
|
|
|
} else {
|
|
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
|
|
State.yawddot = (State.momentX - State.I11dot * PrevState.yawdot +
|
|
State.I22 * PrevState.pitchdot * PrevState.rolldot -
|
|
State.I33 * PrevState.pitchdot * PrevState.rolldot) /
|
|
State.I11;
|
|
State.pitchddot = (State.momentY - State.I22dot * PrevState.pitchdot -
|
|
State.I11 * PrevState.rolldot * PrevState.yawdot +
|
|
State.I33 * PrevState.rolldot * PrevState.yawdot) /
|
|
State.I22;
|
|
State.rollddot = (State.momentZ - State.I33dot * PrevState.rolldot +
|
|
State.I11 * PrevState.pitchdot * PrevState.yawdot -
|
|
State.I22 * PrevState.pitchdot * PrevState.yawdot) /
|
|
State.I33;
|
|
|
|
// p, q, r
|
|
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);
|
|
|
|
// Euler Angles
|
|
State.phidot =
|
|
State.yawdot + (sin(State.pitch) * (State.rolldot * cos(State.yaw) +
|
|
State.pitchdot * sin(State.yaw))) /
|
|
cos(State.pitch);
|
|
State.thetadot =
|
|
State.pitchdot * cos(State.yaw) - State.rolldot * sin(State.yaw);
|
|
State.psidot =
|
|
(State.rolldot * cos(State.yaw) + State.pitchdot * sin(State.yaw)) /
|
|
cos(State.pitch);
|
|
|
|
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);
|
|
|
|
// ax ay az
|
|
State.ax = (State.Fx / State.mass);
|
|
State.ay = (State.Fy / State.mass);
|
|
State.az = (State.Fz / State.mass);
|
|
|
|
// vx vy vz in Earth frame
|
|
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
|
|
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);
|
|
}
|
|
}
|
|
|
|
void pidController(Vehicle &State, struct Vehicle &PrevState) {
|
|
// Make sure we start reacting when we start burning
|
|
if (State.thrust > 0.01) {
|
|
|
|
State.yError = State.yaw;
|
|
State.pError = State.pitch;
|
|
|
|
// Integral of Error
|
|
State.i_yError = integral(State.yError, State.i_yError, State.stepSize);
|
|
State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
|
|
|
|
// Derivative of Error
|
|
State.d_yError = derivative(State.yError, PrevState.yError, State.stepSize);
|
|
State.d_pError = derivative(State.pError, PrevState.pError, State.stepSize);
|
|
|
|
// TVC block properly
|
|
|
|
State.PIDx = (State.Kp * State.yError + State.Ki * State.i_yError +
|
|
State.Kd * State.d_yError) /
|
|
State.momentArm;
|
|
State.PIDy = (State.Kp * State.pError + State.Ki * State.i_pError +
|
|
State.Kd * State.d_pError) /
|
|
State.momentArm;
|
|
|
|
} else {
|
|
State.PIDx = 0;
|
|
State.PIDy = 0;
|
|
}
|
|
|
|
// PID Force Limiter
|
|
State.PIDx = limit(State.PIDx, State.thrust, -State.thrust);
|
|
State.PIDy = limit(State.PIDy, State.thrust, -State.thrust);
|
|
}
|
|
|
|
void TVC(Vehicle &State, Vehicle &PrevState) {
|
|
if (State.thrust < 0.1) {
|
|
// Define forces and moments for t = 0
|
|
State.Fx = 0;
|
|
State.Fy = 0;
|
|
State.Fz = g * State.massInitial;
|
|
|
|
State.momentX = 0;
|
|
State.momentY = 0;
|
|
State.momentZ = 0;
|
|
|
|
} else {
|
|
// Convert servo position to degrees for comparison to max allowable
|
|
State.xServoDegs = (180 / M_PI) * asin(State.PIDx / State.thrust);
|
|
State.yServoDegs = (180 / M_PI) * asin(State.PIDy / State.thrust);
|
|
|
|
// Limit Servo Position
|
|
State.xServoDegs = limit(State.xServoDegs, State.maxServo, -State.maxServo);
|
|
State.yServoDegs = limit(State.yServoDegs, State.maxServo, -State.maxServo);
|
|
|
|
// Servo Rate
|
|
State.xServoDegsDot =
|
|
derivative(State.xServoDegs, PrevState.xServoDegs, State.stepSize);
|
|
State.yServoDegsDot =
|
|
derivative(State.yServoDegs, PrevState.yServoDegs, State.stepSize);
|
|
|
|
// Limit Servo Rate
|
|
State.xServoDegsDot =
|
|
limit(State.xServoDegsDot, State.maxServoRate, -State.maxServoRate);
|
|
State.yServoDegsDot =
|
|
limit(State.yServoDegsDot, State.maxServoRate, -State.maxServoRate);
|
|
|
|
// Back to Degrees
|
|
State.xServoDegs =
|
|
integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize);
|
|
State.yServoDegs =
|
|
integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize);
|
|
}
|
|
}
|
|
|
|
void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
|
|
int t = State.time;
|
|
|
|
stateVector.x[t] = State.x;
|
|
stateVector.y[t] = State.y;
|
|
stateVector.z[t] = State.z;
|
|
|
|
stateVector.vx[t] = State.vx;
|
|
stateVector.vy[t] = State.vy;
|
|
stateVector.vz[t] = State.vz;
|
|
|
|
stateVector.ax[t] = State.ax;
|
|
stateVector.ay[t] = State.ay;
|
|
stateVector.az[t] = State.az;
|
|
|
|
stateVector.yaw[t] = State.yaw;
|
|
stateVector.pitch[t] = State.pitch;
|
|
stateVector.roll[t] = State.roll;
|
|
|
|
stateVector.yawdot[t] = State.yawdot;
|
|
stateVector.pitchdot[t] = State.pitchdot;
|
|
stateVector.rolldot[t] = State.rolldot;
|
|
|
|
stateVector.servo1[t] = State.xServoDegs;
|
|
stateVector.servo2[t] = State.yServoDegs;
|
|
|
|
stateVector.thrustFiring[t] = State.thrustFiring;
|
|
|
|
stateVector.PIDx[t] = State.PIDx;
|
|
stateVector.PIDy[t] = State.PIDy;
|
|
|
|
stateVector.thrust[t] = State.thrust;
|
|
}
|
|
|
|
double derivative(double current, double previous, double step) {
|
|
double dxdt = (current - previous) / (step / 1000);
|
|
return dxdt;
|
|
}
|
|
|
|
double integral(double currentChange, double prevValue, double dt) {
|
|
return (currentChange * dt / 1000) + prevValue;
|
|
}
|
|
|
|
double limit(double value, double upr, double lwr) {
|
|
if (value > upr)
|
|
value = upr;
|
|
else if (value < lwr)
|
|
value = lwr;
|
|
else
|
|
value = value;
|
|
|
|
return value;
|
|
} |