UAVFlightSimulator/main.cpp at master · parthp08/UAVFlightSimulator · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
#include <iostream>
#include <fstream>
#include "Aircraft.h"
#include "PID.h"

constexpr double PI = 3.14;
constexpr double DEGREE2RAD = 0.01745;

void print_vector(std::vector<double>& vec) {
	for (double& i : vec) std::cout << "\t" << i;
	std::cout << std::endl;
}


int main() {

	std::cout << "\t WELCOME TO FLIGHT SIMULATION \t\n";

	// inital states
	// states =>			      u,    v,   w,  p,   q,   r,    e0, e1,  e2,  e3,  Xe,  Ye,   Ze
	std::vector<double> states { 17.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, -100.0 };

	// intialise aircraft from intial states
	Aircraft uav(states);

	// simulation parameters
	double t_current = 0.0;
	double t_end = 60.0;
	double t_step = 1e-2;

	//// altitude hold controller setup
	//PID altitude_hold_controller;
	//const double Kp = 0.2;
	//const double Ki = 0.05;
	//const double Kd = 0.06;
	//altitude_hold_controller.set_gains(Kp, Ki, Kd);
	//altitude_hold_controller.set_input_range(45 * PI / 180.0, -45 * PI / 180.0);

		// intial controls
	double throttle = 0.5, evelvator = 0.0, aileron = 0.0, rudder = 0.0;
	uav.set_control_inputs(throttle, evelvator, aileron, rudder);

	const double desired_velocity = 20.0;
	const double desired_flight_angle = 0.0 * DEGREE2RAD; // rad
	const double desired_altitude = 100.0;
	const double desired_turning_radius = 100.0; // 0.0 => no turn
	//uav.trim(desired_velocity, desired_flight_angle, desired_altitude, desired_turning_radius);

	//// trim states and controls
	//std::vector<double> Xstar, Ustar;
	//Xstar = uav.get_states();
	//Ustar = uav.get_controls();

	//std::cout << "\ttrim states : ";
	//for (auto i : Xstar) { std::cout << "\t" << i; }
	//std::cout << std::endl;

	//std::cout << "\ttrim controls : ";
	//for (auto i : Ustar) { std::cout << "\t" << i; }
	//std::cout << std::endl;

	//std::cout << " - Initiating Flight Simulation" << std::endl;

	//std::cout << "verification of trim : " << std::endl;
	//std::vector<double> XdotStar = uav.calculate_derivatives(Xstar, Ustar);
	//double VaStar = sqrt(Xstar[0] * Xstar[0] + Xstar[1] * Xstar[1] + Xstar[2] * Xstar[2]);
	//double vStar = Xstar[1];
	//std::vector<double> eulerStar = uav.quat2euler(Xstar[6], Xstar[7], Xstar[8], Xstar[9]);
	//double phiStar = eulerStar[0];
	//double thetaStar = eulerStar[1];
	//double psiStar = eulerStar[2];
	//double gamStar = thetaStar - (atan2(Xstar[2], Xstar[0]));

	//std::cout << "XdotStar = ";
	//print_vector(XdotStar);
	//std::cout << "VaStar = " << VaStar << std::endl;
	//std::cout << "gamStar = " << gamStar << std::endl;
	//std::cout << "vStar = " << vStar << std::endl;
	//std::cout << "phiStar = " << phiStar << std::endl;
	//std::cout << "thetaStar = " << thetaStar << std::endl;
	//std::cout << "psiStar = " << psiStar << std::endl;

	// Linearisation -- Longitudinal Plane
	//uav.linearize_lon(desired_velocity, desired_flight_angle, desired_altitude);

	// saved data to save time
	//  --- NEED SET STATE FUNCTION TO APPLY THESE ----------------
	//std::vector<double> Xstar{ 24.969, 0.0, 1.24376, 0.0, 0.0, 0.0, 0.99969, 0.0, 0.0248829, 0.0, 39.2636, -0.332861, -100 };
	//std::vector<double> Ustar{ 0.757636, -0.124113, 0.0, 0.0 };

	// linearization
	//uav.linearize_lon_from_trim(Xstar, Ustar);
	//uav.linearize(Xstar, Ustar);

	uav.trim(17, 0.0 * DEGREE2RAD, 0.0, 0.0);
	auto x_trim = uav.get_states();
	auto u_trim = uav.get_controls();

	uav.linearize(x_trim, u_trim);


	// //Write data to file stream
	//std::ofstream output_stream;

	//while (t_current <= t_end && uav.get_altitude() > 0.0) {
	//
	//	uav.update(t_step);

	//	//elevator = altitude_hold_controller.calculate(-desired_altitude, -uav.get_altitude());
	//	//uav.set_control_inputs(throttle, elevator, aileron, rudder);

	//	//if (t_current >= 10 && t_current <= 11) {
	//	//	uav.set_control_inputs(Ustar[0], Ustar[1]+0.2, Ustar[2], Ustar[3]);
	//	//}
	//	//else if (t_current > 11 && t_current <= 12) {
	//	//	uav.set_control_inputs(Ustar[0], Ustar[1]-0.2, Ustar[2], Ustar[3]);
	//	//}
	//	//else {
	//	//	uav.set_control_inputs(Ustar[0], Ustar[1], Ustar[2], Ustar[3]);
	//	//}

	//	uav.write_to_file(output_stream);

	//	t_current += t_step;
	//};

	//output_stream.close();
	//std::cout << "Flight simultaion complete, please run 'plots.py' in separate console to see the outputs.\n";


	return 0;
}