mirror of
https://gitlab.com/lander-team/lander-cpp.git
synced 2025-06-16 15:17:23 +00:00
initial commit
This commit is contained in:
bpmcgeeney
parent
e2ef9a7232
commit
44d0db8bfd
6
.vscode/settings.json
vendored
Normal file
6
.vscode/settings.json
vendored
Normal file
@ -0,0 +1,6 @@
|
|||||||
|
{
|
||||||
|
"files.associations": {
|
||||||
|
"array": "cpp",
|
||||||
|
"iostream": "cpp"
|
||||||
|
}
|
||||||
|
}
|
||||||
52
.vscode/tasks.json
vendored
Normal file
52
.vscode/tasks.json
vendored
Normal file
@ -0,0 +1,52 @@
|
|||||||
|
{
|
||||||
|
"version": "2.0.0",
|
||||||
|
"tasks": [
|
||||||
|
{
|
||||||
|
"type": "cppbuild",
|
||||||
|
"label": "Debug",
|
||||||
|
"command": "C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe",
|
||||||
|
"args": [
|
||||||
|
"-g",
|
||||||
|
"${workspaceFolder}\\src\\*.cpp",
|
||||||
|
"-o",
|
||||||
|
"${workspaceFolder}\\build\\debug\\${fileBasenameNoExtension}.exe",
|
||||||
|
"-I",
|
||||||
|
"${workspaceFolder}\\include",
|
||||||
|
],
|
||||||
|
"options": {
|
||||||
|
"cwd": "${fileDirname}"
|
||||||
|
},
|
||||||
|
"problemMatcher": [
|
||||||
|
"$gcc"
|
||||||
|
],
|
||||||
|
"group": {
|
||||||
|
"kind": "build",
|
||||||
|
"isDefault": true
|
||||||
|
},
|
||||||
|
"detail": "compiler: \"C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe\""
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"type": "cppbuild",
|
||||||
|
"label": "Release",
|
||||||
|
"command": "C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe",
|
||||||
|
"args": [
|
||||||
|
"${workspaceFolder}\\src\\*.cpp",
|
||||||
|
"-o",
|
||||||
|
"${workspaceFolder}\\build\\release\\${fileBasenameNoExtension}.exe",
|
||||||
|
"-I",
|
||||||
|
"${workspaceFolder}\\include",
|
||||||
|
],
|
||||||
|
"options": {
|
||||||
|
"cwd": "${fileDirname}"
|
||||||
|
},
|
||||||
|
"problemMatcher": [
|
||||||
|
"$gcc"
|
||||||
|
],
|
||||||
|
"group": {
|
||||||
|
"kind": "build",
|
||||||
|
"isDefault": true
|
||||||
|
},
|
||||||
|
"detail": "compiler: \"C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe\""
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
30
build/debug/LQR.m
Normal file
30
build/debug/LQR.m
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
clear all; clc;
|
||||||
|
|
||||||
|
syms R11 R22 R33
|
||||||
|
R = [R11 0 0;
|
||||||
|
0 R22 0;
|
||||||
|
0 0 R33];
|
||||||
|
|
||||||
|
syms I33
|
||||||
|
Q = eye(6) * I33;
|
||||||
|
|
||||||
|
syms F11 F22 F33
|
||||||
|
F = [F11 0 0;
|
||||||
|
0 F22 0
|
||||||
|
0 0 F33];
|
||||||
|
|
||||||
|
syms G13 G31
|
||||||
|
G = [0 0 G13;
|
||||||
|
0 0 0;
|
||||||
|
G31 0 0];
|
||||||
|
|
||||||
|
syms I11 I22 d
|
||||||
|
I = [I11 0 0;
|
||||||
|
0 I22 0;
|
||||||
|
0 0 I33];
|
||||||
|
|
||||||
|
A = [zeros(3,3), d * eye(3); F, G];
|
||||||
|
B = [zeros(3,3); inv(I)];
|
||||||
|
P = [-Q, -A'] * pinv([A, -B*inv(R)*B']);
|
||||||
|
|
||||||
|
K = simplify((R^-1) * B' * P)
|
||||||
BIN
build/debug/main.exe
Normal file
BIN
build/debug/main.exe
Normal file
Binary file not shown.
4318
build/debug/simOut.csv
Normal file
4318
build/debug/simOut.csv
Normal file
File diff suppressed because it is too large
Load Diff
81
build/debug/simPlot.m
Normal file
81
build/debug/simPlot.m
Normal file
@ -0,0 +1,81 @@
|
|||||||
|
clear all; close all; clc;
|
||||||
|
|
||||||
|
% Read data and transfer to variables
|
||||||
|
T = readmatrix('simOut.csv');
|
||||||
|
t = T(:, 1);
|
||||||
|
x = T(:, 2);
|
||||||
|
y = T(:, 3);
|
||||||
|
z = T(:, 4);
|
||||||
|
vx = T(:, 5);
|
||||||
|
vy = T(:, 6);
|
||||||
|
vz = T(:, 7);
|
||||||
|
ax = T(:, 8);
|
||||||
|
ay = T(:, 9);
|
||||||
|
az = T(:, 10);
|
||||||
|
yaw = T(:, 11);
|
||||||
|
pitch = T(:, 12);
|
||||||
|
roll = T(:, 13);
|
||||||
|
yawdot = T(:, 14);
|
||||||
|
pitchdot = T(:, 15);
|
||||||
|
rolldot = T(:, 16);
|
||||||
|
Servo1 = T(:, 17);
|
||||||
|
Servo2 = T(:, 18);
|
||||||
|
m = T(:, 19);
|
||||||
|
thrust = T(:, 20);
|
||||||
|
|
||||||
|
% Acceleration
|
||||||
|
subplot(3, 1, 1)
|
||||||
|
plot(t, az)
|
||||||
|
title('Acceleration vs Time')
|
||||||
|
xlabel('Time (s)')
|
||||||
|
ylabel('Acceleration (g''s)')
|
||||||
|
|
||||||
|
% Velocity
|
||||||
|
subplot(3, 1, 2)
|
||||||
|
plot(t, vz)
|
||||||
|
title('Velocity vs Time')
|
||||||
|
xlabel('Time (s)')
|
||||||
|
ylabel('Velocity (m/s)')
|
||||||
|
|
||||||
|
% Altitude
|
||||||
|
subplot(3, 1, 3)
|
||||||
|
plot(t, z)
|
||||||
|
title('Altitude vs Time')
|
||||||
|
xlabel('Time (s)')
|
||||||
|
ylabel('Altitude (m)')
|
||||||
|
ylim([0 z(1)+5])
|
||||||
|
saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
|
||||||
|
|
||||||
|
figure(2)
|
||||||
|
|
||||||
|
% Euler Angles
|
||||||
|
subplot(2, 1, 1)
|
||||||
|
plot(t, yaw, pitch, roll)
|
||||||
|
title('Euler Angles vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Euler Angles (deg)')
|
||||||
|
|
||||||
|
% Angular Velocity
|
||||||
|
subplot(2, 1, 2)
|
||||||
|
plot(t, yawdot, pitchdot, rolldot)
|
||||||
|
title('Angular Velocity vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Angular Velocity (deg/s)')
|
||||||
|
saveas(gcf,'outputs/Euler Angles vs Time.png')
|
||||||
|
|
||||||
|
figure(3)
|
||||||
|
|
||||||
|
% Servo 1 Position
|
||||||
|
subplot(2, 1, 1)
|
||||||
|
plot(t, Servo1)
|
||||||
|
title('Servo 1 Position vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Servo 1 Position (rad)')
|
||||||
|
|
||||||
|
% Servo 2 Position
|
||||||
|
subplot(2, 1, 2)
|
||||||
|
plot(t, Servo2)
|
||||||
|
title('Servo 2 Position vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Servo 2 Position (rad)')
|
||||||
|
saveas(gcf,'outputs/Servo Position vs Time.png')
|
||||||
30
build/release/LQR.m
Normal file
30
build/release/LQR.m
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
clear all; clc;
|
||||||
|
|
||||||
|
syms R11 R22 R33
|
||||||
|
R = [R11 0 0;
|
||||||
|
0 R22 0;
|
||||||
|
0 0 R33];
|
||||||
|
|
||||||
|
syms I33
|
||||||
|
Q = eye(6) * I33;
|
||||||
|
|
||||||
|
syms F11 F22 F33
|
||||||
|
F = [F11 0 0;
|
||||||
|
0 F22 0
|
||||||
|
0 0 F33];
|
||||||
|
|
||||||
|
syms G13 G31
|
||||||
|
G = [0 0 G13;
|
||||||
|
0 0 0;
|
||||||
|
G31 0 0];
|
||||||
|
|
||||||
|
syms I11 I22 d
|
||||||
|
I = [I11 0 0;
|
||||||
|
0 I22 0;
|
||||||
|
0 0 I33];
|
||||||
|
|
||||||
|
A = [zeros(3,3), d * eye(3); F, G];
|
||||||
|
B = [zeros(3,3); inv(I)];
|
||||||
|
P = [-Q, -A'] * pinv([A, -B*inv(R)*B']);
|
||||||
|
|
||||||
|
K = simplify((R^-1) * B' * P)
|
||||||
BIN
build/release/main.exe
Normal file
BIN
build/release/main.exe
Normal file
Binary file not shown.
81
build/release/simPlot.m
Normal file
81
build/release/simPlot.m
Normal file
@ -0,0 +1,81 @@
|
|||||||
|
clear all; close all; clc;
|
||||||
|
|
||||||
|
% Read data and transfer to variables
|
||||||
|
T = readmatrix('simOut.csv');
|
||||||
|
t = T(:, 1);
|
||||||
|
x = T(:, 2);
|
||||||
|
y = T(:, 3);
|
||||||
|
z = T(:, 4);
|
||||||
|
vx = T(:, 5);
|
||||||
|
vy = T(:, 6);
|
||||||
|
vz = T(:, 7);
|
||||||
|
ax = T(:, 8);
|
||||||
|
ay = T(:, 9);
|
||||||
|
az = T(:, 10);
|
||||||
|
yaw = T(:, 11);
|
||||||
|
pitch = T(:, 12);
|
||||||
|
roll = T(:, 13);
|
||||||
|
yawdot = T(:, 14);
|
||||||
|
pitchdot = T(:, 15);
|
||||||
|
rolldot = T(:, 16);
|
||||||
|
Servo1 = T(:, 17);
|
||||||
|
Servo2 = T(:, 18);
|
||||||
|
m = T(:, 19);
|
||||||
|
thrust = T(:, 20);
|
||||||
|
|
||||||
|
% Acceleration
|
||||||
|
subplot(3, 1, 1)
|
||||||
|
plot(t, az)
|
||||||
|
title('Acceleration vs Time')
|
||||||
|
xlabel('Time (s)')
|
||||||
|
ylabel('Acceleration (g''s)')
|
||||||
|
|
||||||
|
% Velocity
|
||||||
|
subplot(3, 1, 2)
|
||||||
|
plot(t, vz)
|
||||||
|
title('Velocity vs Time')
|
||||||
|
xlabel('Time (s)')
|
||||||
|
ylabel('Velocity (m/s)')
|
||||||
|
|
||||||
|
% Altitude
|
||||||
|
subplot(3, 1, 3)
|
||||||
|
plot(t, z)
|
||||||
|
title('Altitude vs Time')
|
||||||
|
xlabel('Time (s)')
|
||||||
|
ylabel('Altitude (m)')
|
||||||
|
ylim([0 z(1)+5])
|
||||||
|
saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
|
||||||
|
|
||||||
|
figure(2)
|
||||||
|
|
||||||
|
% Euler Angles
|
||||||
|
subplot(2, 1, 1)
|
||||||
|
plot(t, yaw, pitch, roll)
|
||||||
|
title('Euler Angles vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Euler Angles (deg)')
|
||||||
|
|
||||||
|
% Angular Velocity
|
||||||
|
subplot(2, 1, 2)
|
||||||
|
plot(t, yawdot, pitchdot, rolldot)
|
||||||
|
title('Angular Velocity vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Angular Velocity (deg/s)')
|
||||||
|
saveas(gcf,'outputs/Euler Angles vs Time.png')
|
||||||
|
|
||||||
|
figure(3)
|
||||||
|
|
||||||
|
% Servo 1 Position
|
||||||
|
subplot(2, 1, 1)
|
||||||
|
plot(t, Servo1)
|
||||||
|
title('Servo 1 Position vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Servo 1 Position (rad)')
|
||||||
|
|
||||||
|
% Servo 2 Position
|
||||||
|
subplot(2, 1, 2)
|
||||||
|
plot(t, Servo2)
|
||||||
|
title('Servo 2 Position vs Time')
|
||||||
|
xlabel('Time (ms)')
|
||||||
|
ylabel('Servo 2 Position (rad)')
|
||||||
|
saveas(gcf,'outputs/Servo Position vs Time.png')
|
||||||
70
include/sVars.h
Normal file
70
include/sVars.h
Normal file
@ -0,0 +1,70 @@
|
|||||||
|
#include <array>
|
||||||
|
|
||||||
|
#ifndef SVARS_H
|
||||||
|
#define SVARS_H
|
||||||
|
|
||||||
|
struct sVars
|
||||||
|
{
|
||||||
|
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 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 m, m0, mp, mb, mdot;
|
||||||
|
double vehicleHeight, vehicleRadius;
|
||||||
|
|
||||||
|
double tb;
|
||||||
|
double vb;
|
||||||
|
double thrust, thrust_prev, 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;
|
||||||
|
double xServoDegs, yServoDegs;
|
||||||
|
|
||||||
|
double simTime;
|
||||||
|
int stepSize;
|
||||||
|
|
||||||
|
std::array<std::array<double, 2>, 26> thrustCurve = { { {0.148, 7.638},
|
||||||
|
{0.228, 12.253},
|
||||||
|
{0.294, 16.391},
|
||||||
|
{0.353, 20.210},
|
||||||
|
{0.382, 22.756},
|
||||||
|
{0.419, 25.260},
|
||||||
|
{0.477, 23.074},
|
||||||
|
{0.520, 20.845},
|
||||||
|
{0.593, 19.093},
|
||||||
|
{0.688, 17.500},
|
||||||
|
{0.855, 16.225},
|
||||||
|
{1.037, 15.427},
|
||||||
|
{1.205, 14.948},
|
||||||
|
{1.423, 14.627},
|
||||||
|
{1.452, 15.741},
|
||||||
|
{1.503, 14.785},
|
||||||
|
{1.736, 14.623},
|
||||||
|
{1.955, 14.303},
|
||||||
|
{2.210, 14.141},
|
||||||
|
{2.494, 13.819},
|
||||||
|
{2.763, 13.338},
|
||||||
|
{3.120, 13.334},
|
||||||
|
{3.382, 13.013},
|
||||||
|
{3.404, 9.352},
|
||||||
|
{3.418, 4.895},
|
||||||
|
{3.450, 0.000}} };
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
10
include/sim.h
Normal file
10
include/sim.h
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
#include "sVars.h"
|
||||||
|
|
||||||
|
void burnStartTimeCalc(struct sVars&, double g);
|
||||||
|
void thrustSelection(struct sVars&, int t);
|
||||||
|
void lqrCalc(struct sVars&);
|
||||||
|
void TVC(struct sVars&, double g);
|
||||||
|
void vehicleDynamics(struct sVars&, int t);
|
||||||
|
void write2CSV(struct sVars&, std::fstream& outfile, int t);
|
||||||
|
double derivative(double x2, double x1, double dt);
|
||||||
|
double integral(double x2, double x1, double dt);
|
||||||
48
src/main.cpp
Normal file
48
src/main.cpp
Normal file
@ -0,0 +1,48 @@
|
|||||||
|
#include "sim.h"
|
||||||
|
#include "sVars.h"
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
void sim(struct sVars &);
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
sVars Vars;
|
||||||
|
|
||||||
|
// Initial Velocity
|
||||||
|
Vars.vx = 0; // [m/s]
|
||||||
|
Vars.vy = 0; // [m/s]
|
||||||
|
Vars.vz = 0; // [m/s]
|
||||||
|
|
||||||
|
// Initial YPR
|
||||||
|
Vars.yaw = 0; // [rad]
|
||||||
|
Vars.pitch = 0; // [rad]
|
||||||
|
Vars.roll = 0; // [rad]
|
||||||
|
|
||||||
|
// Initial YPRdot
|
||||||
|
Vars.yawdot = 0; // [rad/s]
|
||||||
|
Vars.pitchdot = 0; // [rad/s]
|
||||||
|
Vars.rolldot = 0; // [rad/s]
|
||||||
|
|
||||||
|
// Servo Limitation
|
||||||
|
Vars.maxServo = 0; // [degs]
|
||||||
|
|
||||||
|
// Vehicle Properties
|
||||||
|
Vars.m0 = 1.2; // [kg]
|
||||||
|
Vars.vehicleHeight = 0.5318; // [m]
|
||||||
|
Vars.vehicleRadius = 0.05105; // [m]
|
||||||
|
|
||||||
|
// Sim Step Size
|
||||||
|
Vars.stepSize = 1; // [ms]
|
||||||
|
|
||||||
|
// Other Properties
|
||||||
|
Vars.mp = 0.06; // [kg]
|
||||||
|
Vars.mb = Vars.m0 - Vars.mp; // [kg]
|
||||||
|
Vars.tb = Vars.thrustCurve[25][0] - Vars.thrustCurve[0][0]; // [s]
|
||||||
|
Vars.mdot = Vars.mp / Vars.tb; // [kg/s]
|
||||||
|
|
||||||
|
sim(Vars);
|
||||||
|
|
||||||
|
std::cout << "Finished";
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
403
src/sim.cpp
Normal file
403
src/sim.cpp
Normal file
@ -0,0 +1,403 @@
|
|||||||
|
#include "sim.h"
|
||||||
|
#include <cmath>
|
||||||
|
#include <fstream>
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
void sim(struct sVars &Vars)
|
||||||
|
{
|
||||||
|
double g = -9.81;
|
||||||
|
|
||||||
|
// defining a few random values here cause I'm lazy
|
||||||
|
Vars.burnElapsed = 2000;
|
||||||
|
Vars.m = Vars.m0;
|
||||||
|
Vars.thrust_prev = 0;
|
||||||
|
|
||||||
|
burnStartTimeCalc(Vars, g);
|
||||||
|
|
||||||
|
// Deleting any previous output file
|
||||||
|
if (remove("simOut.csv") != 0)
|
||||||
|
perror("Error deleting file");
|
||||||
|
else
|
||||||
|
puts("File successfully deleted");
|
||||||
|
|
||||||
|
// Define and open output file "simOut.csv"
|
||||||
|
std::fstream outfile;
|
||||||
|
outfile.open("simOut.csv", std::ios::app);
|
||||||
|
|
||||||
|
// 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, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
|
||||||
|
"LQRx, LQRy"
|
||||||
|
<< std::endl;
|
||||||
|
|
||||||
|
// Start Sim
|
||||||
|
for (int t = 0; t < Vars.simTime; t++)
|
||||||
|
{
|
||||||
|
thrustSelection(Vars, t);
|
||||||
|
lqrCalc(Vars);
|
||||||
|
TVC(Vars, g);
|
||||||
|
vehicleDynamics(Vars, t);
|
||||||
|
write2CSV(Vars, outfile, t);
|
||||||
|
}
|
||||||
|
|
||||||
|
outfile.close();
|
||||||
|
}
|
||||||
|
|
||||||
|
void burnStartTimeCalc(struct sVars &Vars, double g)
|
||||||
|
{
|
||||||
|
double v_prev = Vars.vz;
|
||||||
|
double h_prev = 0;
|
||||||
|
double dt, a, v, h;
|
||||||
|
|
||||||
|
for (int i = 0; i < 25; i++)
|
||||||
|
{
|
||||||
|
// Integral of thrust curve
|
||||||
|
if (i < 25)
|
||||||
|
{
|
||||||
|
dt = Vars.thrustCurve[i + 1][0] - Vars.thrustCurve[i][0];
|
||||||
|
a = (Vars.thrustCurve[i][1] /
|
||||||
|
(Vars.m0 - Vars.mdot * Vars.thrustCurve[i + 1][0]) +
|
||||||
|
g);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
dt = 0;
|
||||||
|
a = (Vars.thrustCurve[i][1] / Vars.mb) + g;
|
||||||
|
}
|
||||||
|
|
||||||
|
v = a * dt + v_prev;
|
||||||
|
v_prev = v;
|
||||||
|
h = v * dt + h_prev;
|
||||||
|
h_prev = h;
|
||||||
|
}
|
||||||
|
|
||||||
|
double hb = h; // height that we want to start our burn at
|
||||||
|
double hf =
|
||||||
|
(v * v) / (2 * -g); // amount of height that we need to be in freefall in
|
||||||
|
// order to reach terminal velocity
|
||||||
|
Vars.z = hb + hf; // starting height
|
||||||
|
Vars.vb = v; // terminal velocity
|
||||||
|
|
||||||
|
double burnStartTime = Vars.vb / -g;
|
||||||
|
Vars.simTime = (Vars.tb + burnStartTime) * 1000;
|
||||||
|
}
|
||||||
|
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
// Start burn
|
||||||
|
Vars.burnStart = t;
|
||||||
|
Vars.burnElapsed = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
else
|
||||||
|
Vars.burnElapsed =
|
||||||
|
2000; // 2000 just an arbitrary number to make sure we don't burn
|
||||||
|
|
||||||
|
bool b_thrustSelect;
|
||||||
|
|
||||||
|
for (int i = 0; i < 25; i++)
|
||||||
|
{
|
||||||
|
// Compare elapsed burn time to each time entry in the thrust curve
|
||||||
|
b_thrustSelect = (Vars.burnElapsed < (1 + tol) * Vars.thrustCurve[i][0]) &
|
||||||
|
(Vars.burnElapsed > (1 - tol) * Vars.thrustCurve[i][0]);
|
||||||
|
|
||||||
|
if (b_thrustSelect)
|
||||||
|
{
|
||||||
|
// Choose thrust associated with time entry that elapsed burn time was
|
||||||
|
// compared to above
|
||||||
|
Vars.thrust = Vars.thrustCurve[i][1];
|
||||||
|
Vars.thrust_prev = Vars.thrustCurve[i][1];
|
||||||
|
}
|
||||||
|
|
||||||
|
else
|
||||||
|
// latch
|
||||||
|
Vars.thrust = Vars.thrust_prev;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
double n = sqrt(398600 / pow(6678, 3));
|
||||||
|
double k1 = (Vars.I22 - Vars.I33) / Vars.I11;
|
||||||
|
double k2 = (Vars.I11 - Vars.I33) / Vars.I22;
|
||||||
|
double k3 = (Vars.I22 - Vars.I11) / Vars.I33;
|
||||||
|
|
||||||
|
double R11 = pow(10, 2);
|
||||||
|
double R22 = pow(10, 2);
|
||||||
|
double R33 = pow(10, 2);
|
||||||
|
|
||||||
|
double F11 = -2 * pow(n, 2) * 4 * k1;
|
||||||
|
double F22 = -2 * pow(n, 2) * 3 * k2;
|
||||||
|
double F33 = -2 * pow(n, 2) * k3;
|
||||||
|
|
||||||
|
double G31 = n * (1 - k1);
|
||||||
|
double G13 = n * (k3 - 1);
|
||||||
|
|
||||||
|
double d = 0.5;
|
||||||
|
/*
|
||||||
|
The following calculations are based on output of LQR.m to avoid working
|
||||||
|
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;
|
||||||
|
|
||||||
|
double K11 =
|
||||||
|
-1 * gain *
|
||||||
|
(Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) +
|
||||||
|
R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) /
|
||||||
|
(R11 * d * Vars.I11 *
|
||||||
|
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
|
||||||
|
double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22);
|
||||||
|
double K33 =
|
||||||
|
-1 * gain *
|
||||||
|
(Vars.I33 *
|
||||||
|
(Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) +
|
||||||
|
R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) /
|
||||||
|
(R33 * d * pow(abs(Vars.I33), 2) *
|
||||||
|
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
|
||||||
|
double K34 =
|
||||||
|
gain * (R11 * pow(abs(Vars.I11), 2) * G13) /
|
||||||
|
(R33 * Vars.I33 *
|
||||||
|
(pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
|
||||||
|
double K16 =
|
||||||
|
gain * (R33 * pow(abs(Vars.I33), 2) * G31) /
|
||||||
|
(R11 * Vars.I11 *
|
||||||
|
(pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
|
||||||
|
|
||||||
|
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
|
||||||
|
double momentArm = 0.145;
|
||||||
|
Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / momentArm;
|
||||||
|
Vars.LQRy = (K22 * Vars.pitch / 2) / momentArm;
|
||||||
|
}
|
||||||
|
|
||||||
|
void TVC(struct sVars &Vars, double g)
|
||||||
|
{
|
||||||
|
if (Vars.thrust < 1)
|
||||||
|
{
|
||||||
|
// Define forces and moments for t = 0
|
||||||
|
Vars.Fx = 0;
|
||||||
|
Vars.Fy = 0;
|
||||||
|
Vars.Fz = g * Vars.m0;
|
||||||
|
|
||||||
|
Vars.momentX = 0;
|
||||||
|
Vars.momentY = 0;
|
||||||
|
Vars.momentZ = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Convert servo position to degrees for comparison to max allowable
|
||||||
|
Vars.xServoDegs = (180 / 3.1416) * asin(Vars.LQRx / Vars.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;
|
||||||
|
|
||||||
|
// Convert servo position to degrees for comparison to max allowable
|
||||||
|
Vars.yServoDegs = (180 / 3.1416) * asin(Vars.LQRy / Vars.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;
|
||||||
|
|
||||||
|
// Vector math to aqcuire thrust vector components
|
||||||
|
Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (3.1416 / 180));
|
||||||
|
Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (3.1416 / 180));
|
||||||
|
Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) +
|
||||||
|
(Vars.m * g);
|
||||||
|
|
||||||
|
// Calculate moment created by Fx and Fy
|
||||||
|
double momentArm = 0.145;
|
||||||
|
Vars.momentX = Vars.Fx * momentArm;
|
||||||
|
Vars.momentY = Vars.Fy * momentArm;
|
||||||
|
Vars.momentZ = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void vehicleDynamics(struct sVars &Vars, int t)
|
||||||
|
{
|
||||||
|
// Idot
|
||||||
|
if (t < 1)
|
||||||
|
{
|
||||||
|
Vars.I11dot = 0;
|
||||||
|
Vars.I22dot = 0;
|
||||||
|
Vars.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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
if (t < 1)
|
||||||
|
{
|
||||||
|
Vars.x = 0;
|
||||||
|
Vars.y = 0;
|
||||||
|
|
||||||
|
Vars.ax = 0;
|
||||||
|
Vars.ay = 0;
|
||||||
|
Vars.az = Vars.Fz / Vars.m0;
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set "prev" values for next timestep
|
||||||
|
Vars.I11prev = Vars.I11;
|
||||||
|
Vars.I22prev = Vars.I22;
|
||||||
|
Vars.I33prev = Vars.I33;
|
||||||
|
|
||||||
|
Vars.yawPrev = Vars.yaw;
|
||||||
|
Vars.pitchPrev = Vars.pitch;
|
||||||
|
Vars.rollPrev = Vars.roll;
|
||||||
|
|
||||||
|
Vars.yawdotPrev = Vars.yawdot;
|
||||||
|
Vars.pitchdotPrev = Vars.pitchdot;
|
||||||
|
Vars.rolldotPrev = Vars.rolldot;
|
||||||
|
|
||||||
|
Vars.yawddotPrev = Vars.yawddot;
|
||||||
|
Vars.pitchddotPrev = Vars.pitchddot;
|
||||||
|
Vars.rollddotPrev = Vars.rollddot;
|
||||||
|
|
||||||
|
Vars.axPrev = Vars.ax;
|
||||||
|
Vars.ayPrev = Vars.ay;
|
||||||
|
Vars.azPrev = Vars.az;
|
||||||
|
|
||||||
|
Vars.vxPrev = Vars.vx;
|
||||||
|
Vars.vyPrev = Vars.vy;
|
||||||
|
Vars.vzPrev = Vars.vz;
|
||||||
|
|
||||||
|
Vars.xPrev = Vars.x;
|
||||||
|
Vars.yPrev = Vars.y;
|
||||||
|
Vars.zPrev = Vars.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
void write2CSV(struct sVars &Vars, std::fstream &outfile, int t)
|
||||||
|
{
|
||||||
|
// writing to output file
|
||||||
|
outfile << t << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.x << ", ";
|
||||||
|
outfile << Vars.y << ", ";
|
||||||
|
outfile << Vars.z << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.vx << ", ";
|
||||||
|
outfile << Vars.vy << ", ";
|
||||||
|
outfile << Vars.vz << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.ax << ", ";
|
||||||
|
outfile << Vars.ay << ", ";
|
||||||
|
outfile << Vars.az << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.yaw * 180 / 3.1416 << ", ";
|
||||||
|
outfile << Vars.pitch * 180 / 3.1416 << ", ";
|
||||||
|
outfile << Vars.roll * 180 / 3.1416 << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.yawdot * 180 / 3.1416 << ", ";
|
||||||
|
outfile << Vars.pitchdot * 180 / 3.1416 << ", ";
|
||||||
|
outfile << Vars.rolldot * 180 / 3.1416 << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.xServoDegs << ", ";
|
||||||
|
outfile << Vars.yServoDegs << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.m << ", ";
|
||||||
|
outfile << Vars.thrust << ", ";
|
||||||
|
outfile << Vars.burnElapsed << ", ";
|
||||||
|
outfile << Vars.Fz << ", ";
|
||||||
|
|
||||||
|
outfile << Vars.LQRx << ", ";
|
||||||
|
outfile << Vars.LQRy << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
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 integ = (x * dt / 1000) + y;
|
||||||
|
return integ;
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user