mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 23:26:43 +00:00
reformatted code
This commit is contained in:
parent
39abd73eb4
commit
24bf3f2102
@ -3,8 +3,7 @@
|
||||
#ifndef SVARS_H
|
||||
#define SVARS_H
|
||||
|
||||
struct sVars
|
||||
{
|
||||
struct sVars {
|
||||
double x, y, z;
|
||||
double xPrev, yPrev, zPrev;
|
||||
double vx, vy, vz;
|
||||
|
||||
@ -1,11 +1,10 @@
|
||||
#include "sim.h"
|
||||
#include "sVars.h"
|
||||
#include "sim.h"
|
||||
#include <iostream>
|
||||
|
||||
void sim(struct sVars &);
|
||||
|
||||
int main()
|
||||
{
|
||||
int main() {
|
||||
sVars Vars;
|
||||
|
||||
// Initial Velocity
|
||||
|
||||
90
src/sim.cpp
90
src/sim.cpp
@ -4,8 +4,7 @@
|
||||
#include <iostream>
|
||||
#include <stdio.h>
|
||||
|
||||
void sim(struct sVars &Vars)
|
||||
{
|
||||
void sim(struct sVars &Vars) {
|
||||
double g = -9.81;
|
||||
|
||||
// defining a few random values here cause I'm lazy
|
||||
@ -27,14 +26,15 @@ void sim(struct sVars &Vars)
|
||||
|
||||
// Output file header. These are the variables that we output - useful for
|
||||
// debugging
|
||||
outfile << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
|
||||
"pitchdot, rolldot, yawddot, pitchddot, rollddot, I11, I22, I33, I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
|
||||
outfile
|
||||
<< "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
|
||||
"pitchdot, rolldot, yawddot, pitchddot, rollddot, I11, I22, I33, "
|
||||
"I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
|
||||
"LQRx, LQRy"
|
||||
<< std::endl;
|
||||
|
||||
// Start Sim
|
||||
for (int t = 0; t < Vars.simTime; t++)
|
||||
{
|
||||
for (int t = 0; t < Vars.simTime; t++) {
|
||||
thrustSelection(Vars, t);
|
||||
lqrCalc(Vars);
|
||||
TVC(Vars, g);
|
||||
@ -45,22 +45,21 @@ void sim(struct sVars &Vars)
|
||||
outfile.close();
|
||||
}
|
||||
|
||||
void burnStartTimeCalc(struct sVars &Vars, double g)
|
||||
{
|
||||
void burnStartTimeCalc(struct sVars &Vars, double g) {
|
||||
double v = Vars.vz;
|
||||
double h = 0;
|
||||
double dt = 0.001;
|
||||
double a, j, m, thrust;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
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;
|
||||
@ -76,24 +75,21 @@ void burnStartTimeCalc(struct sVars &Vars, double g)
|
||||
Vars.simTime = (Vars.tb + burnStartTime) * 1000;
|
||||
}
|
||||
|
||||
void thrustSelection(struct sVars &Vars, int t)
|
||||
{
|
||||
void thrustSelection(struct sVars &Vars, int t) {
|
||||
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);
|
||||
|
||||
if (Vars.burnElapsed != 2000)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
else if (b_burnStart)
|
||||
{
|
||||
else if (b_burnStart) {
|
||||
// Start burn
|
||||
Vars.burnStart = t;
|
||||
Vars.burnElapsed = 0;
|
||||
@ -106,14 +102,16 @@ void thrustSelection(struct sVars &Vars, int t)
|
||||
Vars.thrust = 65.165 * Vars.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;
|
||||
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 ((Vars.burnElapsed > 3.382) & (Vars.burnElapsed < 3.46))
|
||||
Vars.thrust = -195.78 * Vars.burnElapsed + 675.11;
|
||||
}
|
||||
|
||||
void lqrCalc(struct sVars &Vars)
|
||||
{
|
||||
void lqrCalc(struct sVars &Vars) {
|
||||
|
||||
Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
|
||||
pow(Vars.vehicleRadius, 2) / 4);
|
||||
@ -145,7 +143,7 @@ void lqrCalc(struct sVars &Vars)
|
||||
with matrices in C++ Please see .m file for details on calculation.
|
||||
Algorithm taken from LQR wikipedia page:
|
||||
https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
|
||||
*/
|
||||
*/
|
||||
/*
|
||||
double gain = 0.25;
|
||||
|
||||
@ -198,11 +196,21 @@ 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
|
||||
double gain =
|
||||
0.25 *
|
||||
pow(10, -4); // changing exponenet drastically changes results of LQR
|
||||
|
||||
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
|
||||
Vars.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 = gain * ((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 + K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) / -Vars.momentArm;
|
||||
Vars.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 =
|
||||
gain *
|
||||
((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 +
|
||||
K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) /
|
||||
-Vars.momentArm;
|
||||
|
||||
// LQR Force limiter X
|
||||
if (Vars.LQRx > Vars.thrust)
|
||||
@ -217,10 +225,8 @@ void lqrCalc(struct sVars &Vars)
|
||||
Vars.LQRy = -1 * Vars.thrust;
|
||||
}
|
||||
|
||||
void TVC(struct sVars &Vars, double g)
|
||||
{
|
||||
if (Vars.thrust < 1)
|
||||
{
|
||||
void TVC(struct sVars &Vars, double g) {
|
||||
if (Vars.thrust < 1) {
|
||||
// Define forces and moments for t = 0
|
||||
Vars.Fx = 0;
|
||||
Vars.Fy = 0;
|
||||
@ -231,8 +237,7 @@ void TVC(struct sVars &Vars, double g)
|
||||
Vars.momentZ = 0;
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
else {
|
||||
// Convert servo position to degrees for comparison to max allowable
|
||||
Vars.xServoDegs = (180 / 3.1416) * asin(Vars.LQRx / Vars.thrust);
|
||||
|
||||
@ -264,17 +269,13 @@ void TVC(struct sVars &Vars, double g)
|
||||
}
|
||||
}
|
||||
|
||||
void vehicleDynamics(struct sVars &Vars, int t)
|
||||
{
|
||||
void vehicleDynamics(struct sVars &Vars, int t) {
|
||||
// Idot
|
||||
if (t < 1)
|
||||
{
|
||||
if (t < 1) {
|
||||
Vars.I11dot = 0;
|
||||
Vars.I22dot = 0;
|
||||
Vars.I33dot = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
} 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);
|
||||
@ -294,8 +295,7 @@ void vehicleDynamics(struct sVars &Vars, int t)
|
||||
Vars.I22 * Vars.pitchdot * Vars.yawdot) /
|
||||
Vars.I33;
|
||||
|
||||
if (t < 1)
|
||||
{
|
||||
if (t < 1) {
|
||||
Vars.x = 0;
|
||||
Vars.y = 0;
|
||||
|
||||
@ -304,8 +304,7 @@ void vehicleDynamics(struct sVars &Vars, int t)
|
||||
Vars.az = Vars.Fz / Vars.m0;
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
else {
|
||||
// p, q, r
|
||||
Vars.yawdot = integral(Vars.yawddot, Vars.yawdotPrev, Vars.stepSize);
|
||||
Vars.pitchdot = integral(Vars.pitchddot, Vars.pitchdotPrev, Vars.stepSize);
|
||||
@ -374,8 +373,7 @@ void vehicleDynamics(struct sVars &Vars, int t)
|
||||
Vars.zPrev = Vars.z;
|
||||
}
|
||||
|
||||
void write2CSV(struct sVars &Vars, std::fstream &outfile, int t)
|
||||
{
|
||||
void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
|
||||
// writing to output file
|
||||
outfile << t << ", ";
|
||||
|
||||
@ -423,14 +421,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 derivative(double x2, double x1, double dt) {
|
||||
double dxdt = (x2 - x1) / (dt / 1000);
|
||||
return dxdt;
|
||||
}
|
||||
|
||||
double integral(double x, double y, double dt)
|
||||
{
|
||||
double integral(double x, double y, double dt) {
|
||||
double integ = (x * dt / 1000) + y;
|
||||
return integ;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user