Skip to content

Commit d0049c9

Browse files
jediofgeverjediofgever
authored
Add files via upload
1 parent 378570d commit d0049c9

16 files changed

+1853
-0
lines changed

‎.DS_Store

6 KB
Binary file not shown.

‎autopilot.m

+247
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,247 @@
1+
function y = autopilot(uu,P)
2+
%
3+
% autopilot for mavsim
4+
%
5+
% Modification History:
6+
7+
%
8+
9+
% process inputs
10+
NN = 0;
11+
pn = uu(1+NN); % inertial North position
12+
h = uu(2+NN); % altitude
13+
Va = uu(3+NN);
14+
alpha = uu(4+NN);
15+
theta = uu(5+NN); % pitch angle
16+
q = uu(6+NN); % body frame pitch rate
17+
Vg = uu(7+NN); % ground speed
18+
wn = uu(8+NN); % wind North
19+
by = uu(9+NN); % y-gyro bias
20+
NN = NN+9;
21+
Va_c = uu(1+NN); % commanded airspeed (m/s)
22+
h_c = uu(2+NN); % commanded altitude (m)
23+
NN = NN+2;
24+
t = uu(1+NN); % time
25+
26+
[delta, x_command] = autopilot_no_state_machine(Va_c,h_c,Va,h,theta,q,t,P);
27+
y = [delta; x_command];
28+
end
29+
30+
31+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
32+
% autopilot_no_state_machine. Works well for overpowered aerosonde
33+
% - autopilot defined in the uavbook
34+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
35+
function [delta, x_command] = autopilot_no_state_machine(Va_c,h_c,Va,h,theta,q,t,P)
36+
37+
38+
%----------------------------------------------------------
39+
% longitudinal autopilot
40+
if t==0,
41+
delta_t = airspeed_with_throttle_hold(Va_c, Va, 1, P);
42+
theta_c = altitude_hold(h_c, h, 1, P);
43+
else
44+
delta_t = airspeed_with_throttle_hold(Va_c, Va, 0, P);
45+
theta_c = altitude_hold(h_c, h, 0, P);
46+
end
47+
48+
delta_e = pitch_hold(theta_c, theta, q, P);
49+
% artificially saturation delta_t
50+
delta_t = sat(delta_t,1,0);
51+
52+
53+
%----------------------------------------------------------
54+
% create outputs
55+
56+
% control outputs
57+
delta = [delta_e; delta_t];
58+
% commanded (desired) states
59+
x_command = [...
60+
0;... % pn
61+
h_c;... % h
62+
Va_c;... % Va
63+
0;... % alpha
64+
theta_c;
65+
0;... % q
66+
];
67+
68+
y = [delta; x_command];
69+
70+
end
71+
72+
73+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
74+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
75+
% Autopilot functions
76+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
77+
78+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
79+
% pitch_hold
80+
% - regulate pitch using elevator
81+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
82+
function delta_e = pitch_hold(theta_c, theta, q, P)
83+
84+
% compute the current error
85+
error = theta_c - theta;
86+
87+
% proportional term
88+
up = P.pitch_kp * error;
89+
90+
% derivative term
91+
ud = -P.pitch_kd * q;
92+
93+
% implement PID control
94+
delta_e = sat(up + ud, 45*pi/180, -45*pi/180);
95+
96+
end
97+
98+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
99+
% airspeed_with_pitch_hold
100+
% - regulate airspeed using pitch angle
101+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
102+
function theta_c = airspeed_with_pitch_hold(Va_c, Va, flag, P)
103+
persistent integrator;
104+
persistent error_d1;
105+
% initialize persistent variables at beginning of simulation
106+
if flag==1,
107+
integrator = 0;
108+
error_d1 = 0;
109+
end
110+
111+
% compute the current error
112+
error = Va_c - Va;
113+
114+
% update the integrator
115+
integrator = integrator + (P.Ts/2)*(error + error_d1); % trapazoidal rule
116+
117+
% proportional term
118+
up = P.airspeed_pitch_kp * error;
119+
120+
% integral term
121+
ui = P.airspeed_pitch_ki * integrator;
122+
123+
% implement PID control
124+
theta_c = sat(up + ui, P.theta_c_max, -P.theta_c_max);
125+
126+
% implement integrator antiwindup
127+
if P.airspeed_pitch_ki~=0,
128+
theta_c_unsat = up + ui;
129+
k_antiwindup = P.Ts/P.airspeed_pitch_ki;
130+
integrator = integrator + k_antiwindup*(theta_c-theta_c_unsat);
131+
end
132+
133+
% update persistent variables
134+
error_d1 = error;
135+
136+
end
137+
138+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
139+
% airspeed_with_throttle_hold
140+
% - regulate airspeed using throttle
141+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
142+
function delta_t = airspeed_with_throttle_hold(Va_c, Va, flag, P)
143+
persistent integrator;
144+
persistent error_d1;
145+
% initialize persistent variables at beginning of simulation
146+
if flag==1,
147+
integrator = 0;
148+
error_d1 = 0;
149+
end
150+
151+
% compute the current error
152+
error = Va_c - Va;
153+
154+
% update the integrator
155+
integrator = integrator + (P.Ts/2)*(error + error_d1); % trapazoidal rule
156+
157+
% proportional term
158+
up = P.airspeed_throttle_kp * error;
159+
160+
% integral term
161+
ui = P.airspeed_throttle_ki * integrator;
162+
163+
% implement PID control
164+
delta_t = sat(up + ui, 1, 0);
165+
166+
% implement integrator anti-windup
167+
if P.airspeed_throttle_ki~=0,
168+
delta_t_unsat = up + ui;
169+
k_antiwindup = P.Ts/P.airspeed_throttle_ki;
170+
integrator = integrator + k_antiwindup*(delta_t-delta_t_unsat);
171+
end
172+
173+
% update persistent variables
174+
error_d1 = error;
175+
176+
end
177+
178+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
179+
% altitude_hold
180+
% - regulate altitude using pitch angle
181+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
182+
function theta_c = altitude_hold(h_c, h, flag, P)
183+
persistent integrator;
184+
persistent error_d1;
185+
persistent hdot;
186+
persistent hdot_d1;
187+
persistent h_d1;
188+
189+
% initialize persistent variables at beginning of simulation
190+
if flag==1,
191+
integrator = 0;
192+
error_d1 = 0;
193+
hdot = 0;
194+
hdot_d1 = 0;
195+
h_d1 = 0;
196+
end
197+
198+
% compute the current error
199+
error = h_c - h;
200+
201+
% update the integrator
202+
integrator = integrator + (P.Ts/2)*(error + error_d1); % trapazoidal rule
203+
204+
% update the differentiator
205+
hdot = (2*P.tau-P.Ts)/(2*P.tau+P.Ts)*hdot_d1...
206+
+ (2/(2*P.tau+P.Ts))*(h - h_d1);
207+
208+
% proportional term
209+
up = P.altitude_kp * error;
210+
211+
% integral term
212+
ui = P.altitude_ki * integrator;
213+
214+
% derivative gain
215+
ud = P.altitude_kd * hdot;
216+
217+
% implement PID control
218+
theta_c = sat(up + ui + ud, P.theta_c_max, -P.theta_c_max);
219+
220+
% implement integrator anti-windup
221+
if P.altitude_ki~=0,
222+
theta_c_unsat = up + ui + ud;
223+
k_antiwindup = P.Ts/P.altitude_ki;
224+
integrator = integrator + k_antiwindup*(theta_c-theta_c_unsat);
225+
end
226+
227+
% update persistent variables
228+
error_d1 = error;
229+
hdot_d1 = hdot;
230+
h_d1 = h;
231+
end
232+
233+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
234+
% sat
235+
% - saturation function
236+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
237+
function out = sat(in, up_limit, low_limit)
238+
if in > up_limit,
239+
out = up_limit;
240+
elseif in < low_limit;
241+
out = low_limit;
242+
else
243+
out = in;
244+
end
245+
end
246+
247+

‎batcam.png

102 KB
Loading

0 commit comments

Comments
 (0)