Skip to content

Commit 8fb1d71

Browse files
ufechner7ufechner7
committed
Split package in two files
1 parent d732585 commit 8fb1d71

File tree

3 files changed

+109
-106
lines changed

3 files changed

+109
-106
lines changed

‎src/SimpleKiteControllers.jl‎

Lines changed: 2 additions & 106 deletions
Original file line numberDiff line numberDiff line change
@@ -5,112 +5,8 @@ using DiscretePIDs, Parameters, KiteUtils
55
export ParkingController, ParkingControllerSettings
66
export linearize, calc_steering, navigate
77

8-
@with_kw mutable struct ParkingControllerSettings @deftype Float64
9-
dt
10-
# turn rate controller settings
11-
kp_tr=0.06
12-
ki_tr=0.0012
13-
# outer controller (heading/ course) settings
14-
kp=15
15-
ki=0.5
16-
# NDI block settings
17-
va_min = 5.0 # minimum apparent wind speed
18-
va_max = 100.0 # maximum apparent wind speed
19-
k_ds = 2.0 # influence of the depower settings on the steering sensitivity
20-
c1 = 0.048 # v9 kite model
21-
c2 = 0 # a value other than zero creates more problems than it solves
22-
end
23-
24-
mutable struct ParkingController
25-
pcs::ParkingControllerSettings
26-
pid_tr::DiscretePID
27-
pid_outer::DiscretePID
28-
last_heading::Float64
29-
chi_set::Float64
30-
last_ndi_gain::Float64
31-
end
32-
33-
function ParkingController(pcs::ParkingControllerSettings; last_heading = 0.0)
34-
Ts = pcs.dt
35-
pid_tr = DiscretePID(;K=pcs.kp_tr, Ti=pcs.kp_tr/ pcs.ki_tr, Ts)
36-
pid_outer = DiscretePID(;K=pcs.kp, Ti=pcs.kp/ pcs.ki, Ts)
37-
return ParkingController(pcs, pid_tr, pid_outer, last_heading, 0, 0)
38-
end
39-
40-
"""
41-
linearize(pcs::ParkingControllerSettings, psi_dot, psi, elevation, v_app; ud_prime=0)
42-
43-
Nonlinear, dynamic inversion block (NDI) according to Eq. 6.4 and Eq. 6.12.
44-
45-
Parameters:
46-
- psi_dot: desired turn rate in radians per second
47-
- psi: heading in radians
48-
- elevation: elevation angle in radians
49-
- v_app: apparent wind speed in m/s
50-
- ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered
51-
"""
52-
function linearize(pc::ParkingController, psi_dot, psi, elevation, v_app; ud_prime=0)
53-
pcs = pc.pcs
54-
# Eq. 6.13: calculate va_hat
55-
va_hat = clamp(v_app, pcs.va_min, pcs.va_max)
56-
# Eq. 6.12: calculate the steering from the desired turn rate
57-
u_s = (1.0 + pcs.k_ds * ud_prime) / (pcs.c1 * va_hat) * (psi_dot - pcs.c2 / va_hat * sin(psi) * cos(elevation))
58-
if abs(psi_dot) < 1e-6
59-
ndi_gain = pc.last_ndi_gain
60-
else
61-
ndi_gain = clamp(u_s / psi_dot, -20, 20.0)
62-
end
63-
pc.last_ndi_gain = ndi_gain
64-
return u_s, ndi_gain
65-
end
66-
67-
"""
68-
navigate(fpc, limit=50.0)
69-
70-
Calculate the desired flight direction chi_set using great circle navigation.
71-
Limit delta_beta to the value of the parameter limit (in degrees).
72-
"""
73-
function navigate(pc::ParkingController, azimuth, elevation; limit=50.0)
74-
phi_set = 0.0 # azimuth
75-
beta_set = deg2rad(77) # zenith
76-
beta = elevation
77-
phi = azimuth
78-
# println("beta: $(rad2deg(beta)), phi: $(rad2deg(phi))")
79-
r_limit = deg2rad(limit)
80-
if beta_set - beta > r_limit
81-
beta_set = beta + r_limit
82-
elseif beta_set - beta < -r_limit
83-
beta_set = beta - r_limit
84-
end
85-
y = sin(phi_set - phi) * cos(beta_set)
86-
x = cos(beta) * sin(beta_set) - sin(beta) * cos(beta_set) * cos(phi_set - phi)
87-
pc.chi_set = atan(y, x)
88-
end
89-
90-
"""
91-
calc_steering(pc::ParkingController, heading; elevation=0.0, v_app=10.0, ud_prime=0.0)
92-
93-
Calculate rel_steering and ndi_gain from the actual heading, elevation, and apparent wind speed.
94-
95-
Parameters:
96-
- pc: parking controller
97-
- heading: actual heading in radians
98-
- elevation: elevation angle in radians
99-
- v_app: apparent wind speed in m/s
100-
- ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered
101-
102-
"""
103-
function calc_steering(pc::ParkingController, heading, chi_set; elevation=0.0, v_app=10.0, ud_prime=0.0)
104-
# calculate the desired turn rate
105-
heading = wrap2pi(heading) # a different wrap2pi function is needed that avoids any jumps
106-
psi_dot_set = pc.pid_outer(wrap2pi(chi_set), heading)
107-
psi_dot = (wrap2pi(heading - pc.last_heading)) / pc.pcs.dt
108-
pc.last_heading = heading
109-
psi_dot_in = pc.pid_tr(psi_dot_set, psi_dot)
110-
# linearize the NDI block
111-
u_s, ndi_gain = linearize(pc, psi_dot_in, heading, elevation, v_app; ud_prime)
112-
u_s, ndi_gain, psi_dot, psi_dot_set
113-
end
8+
include("parking_controller.jl")
9+
include("figure_eight_controller.jl")
11410

11511
end
11612

‎src/figure_eight_controller.jl‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
# TODO: implement figure of eight controller

‎src/parking_controller.jl‎

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
@with_kw mutable struct ParkingControllerSettings @deftype Float64
2+
dt
3+
# turn rate controller settings
4+
kp_tr=0.06
5+
ki_tr=0.0012
6+
# outer controller (heading/ course) settings
7+
kp=15
8+
ki=0.5
9+
# NDI block settings
10+
va_min = 5.0 # minimum apparent wind speed
11+
va_max = 100.0 # maximum apparent wind speed
12+
k_ds = 2.0 # influence of the depower settings on the steering sensitivity
13+
c1 = 0.048 # v9 kite model
14+
c2 = 0 # a value other than zero creates more problems than it solves
15+
end
16+
17+
mutable struct ParkingController
18+
pcs::ParkingControllerSettings
19+
pid_tr::DiscretePID
20+
pid_outer::DiscretePID
21+
last_heading::Float64
22+
chi_set::Float64
23+
last_ndi_gain::Float64
24+
end
25+
26+
function ParkingController(pcs::ParkingControllerSettings; last_heading = 0.0)
27+
Ts = pcs.dt
28+
pid_tr = DiscretePID(;K=pcs.kp_tr, Ti=pcs.kp_tr/ pcs.ki_tr, Ts)
29+
pid_outer = DiscretePID(;K=pcs.kp, Ti=pcs.kp/ pcs.ki, Ts)
30+
return ParkingController(pcs, pid_tr, pid_outer, last_heading, 0, 0)
31+
end
32+
33+
"""
34+
linearize(pcs::ParkingControllerSettings, psi_dot, psi, elevation, v_app; ud_prime=0)
35+
36+
Nonlinear, dynamic inversion block (NDI) according to Eq. 6.4 and Eq. 6.12.
37+
38+
Parameters:
39+
- psi_dot: desired turn rate in radians per second
40+
- psi: heading in radians
41+
- elevation: elevation angle in radians
42+
- v_app: apparent wind speed in m/s
43+
- ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered
44+
"""
45+
function linearize(pc::ParkingController, psi_dot, psi, elevation, v_app; ud_prime=0)
46+
pcs = pc.pcs
47+
# Eq. 6.13: calculate va_hat
48+
va_hat = clamp(v_app, pcs.va_min, pcs.va_max)
49+
# Eq. 6.12: calculate the steering from the desired turn rate
50+
u_s = (1.0 + pcs.k_ds * ud_prime) / (pcs.c1 * va_hat) * (psi_dot - pcs.c2 / va_hat * sin(psi) * cos(elevation))
51+
if abs(psi_dot) < 1e-6
52+
ndi_gain = pc.last_ndi_gain
53+
else
54+
ndi_gain = clamp(u_s / psi_dot, -20, 20.0)
55+
end
56+
pc.last_ndi_gain = ndi_gain
57+
return u_s, ndi_gain
58+
end
59+
60+
"""
61+
navigate(fpc, limit=50.0)
62+
63+
Calculate the desired flight direction chi_set using great circle navigation.
64+
Limit delta_beta to the value of the parameter limit (in degrees).
65+
"""
66+
function navigate(pc::ParkingController, azimuth, elevation; limit=50.0)
67+
phi_set = 0.0 # azimuth
68+
beta_set = deg2rad(77) # zenith
69+
beta = elevation
70+
phi = azimuth
71+
# println("beta: $(rad2deg(beta)), phi: $(rad2deg(phi))")
72+
r_limit = deg2rad(limit)
73+
if beta_set - beta > r_limit
74+
beta_set = beta + r_limit
75+
elseif beta_set - beta < -r_limit
76+
beta_set = beta - r_limit
77+
end
78+
y = sin(phi_set - phi) * cos(beta_set)
79+
x = cos(beta) * sin(beta_set) - sin(beta) * cos(beta_set) * cos(phi_set - phi)
80+
pc.chi_set = atan(y, x)
81+
end
82+
83+
"""
84+
calc_steering(pc::ParkingController, heading; elevation=0.0, v_app=10.0, ud_prime=0.0)
85+
86+
Calculate rel_steering and ndi_gain from the actual heading, elevation, and apparent wind speed.
87+
88+
Parameters:
89+
- pc: parking controller
90+
- heading: actual heading in radians
91+
- elevation: elevation angle in radians
92+
- v_app: apparent wind speed in m/s
93+
- ud_prime: depower setting in the range of 0 to 1, 0 means fully powered, 1 means fully depowered
94+
95+
"""
96+
function calc_steering(pc::ParkingController, heading, chi_set; elevation=0.0, v_app=10.0, ud_prime=0.0)
97+
# calculate the desired turn rate
98+
heading = wrap2pi(heading) # a different wrap2pi function is needed that avoids any jumps
99+
psi_dot_set = pc.pid_outer(wrap2pi(chi_set), heading)
100+
psi_dot = (wrap2pi(heading - pc.last_heading)) / pc.pcs.dt
101+
pc.last_heading = heading
102+
psi_dot_in = pc.pid_tr(psi_dot_set, psi_dot)
103+
# linearize the NDI block
104+
u_s, ndi_gain = linearize(pc, psi_dot_in, heading, elevation, v_app; ud_prime)
105+
u_s, ndi_gain, psi_dot, psi_dot_set
106+
end

0 commit comments

Comments
 (0)