control_tutorial/vs_comp.py at master · arthurrichards77/control_tutorial · 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
"""
Execute step in vertical speed using (optional) state
space compensator in the control loop
"""
from fgclient import FgClient
c = FgClient()

# check zero elevator
el = c.get_elevator()
if el != 0.0:
    raise ValueError('Elevator not central: ', el)

c.ap_pitch_off()

# compensator definition
comp_a = 0.9950124791926824
comp_b = 0.4987520807317687
comp_c = 0.09000000000000001
comp_d = 1.0
x_comp = 0.0 # initial internal state

kk = 0
dt = 0.5
for kk in range(120):
    c.tic()
    if kk > 10:
        vs_des = 5.0
    else:
        vs_des = 0.0
    vs = c.vertical_speed_fps()
    err = vs_des - vs
    # update compensator
    y_comp = comp_c*x_comp + comp_d*err
    x_comp = comp_a*x_comp + comp_b*err
    # apply feedback
    c.set_elevator(-0.005*y_comp) # with compensator
    #c.set_elevator(-0.005*err) # without compensator
    print(vs)
    c.toc(dt)

c.ap_pitch_vs(0.0)