from fgclient import FgClient
c = FgClient()
hdg_des = c.heading_deg()+15
if hdg_des<0:
hdg_des+=360
if hdg_des>360:
hdg_des-=360
c.ap_pitch_vs()
c.ap_roll_off()
last_hdg = None
int_err = 0.0
kk = 0
while True:
kk+=1
c.tic()
hdg = c.heading_deg()
# P
err = hdg_des - hdg
ail = 0.005*err
# D
if last_hdg:
ail += -0.03*(hdg - last_hdg)/0.5
last_hdg = hdg
# I
int_err += 0.5*err
ail += 0.001*int_err
# out
c.set_aileron(ail)
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)
Start up and take off from simulator reset
"""
from fgclient import FgClient
c = FgClient()
c.set_prop('/controls/engines/engine/magnetos', 3)
c.set_prop('/controls/engines/engine/throttle', 0.75)
c.set_prop('/controls/gear/brake-parking', 0)
c.set_prop('/sim/hud/visibility[0]', 1)
c.set_aileron(0.0)
c.set_elevator(0.0)
c.set_prop('/controls/engines/engine/starter', 1)
for kk in range(30):
c.tic()
sp = c.get_prop_float('/velocities/airspeed-kt')
print(sp)
if sp > 50:
c.set_elevator(-0.1)
c.toc(1.0)
for kk in range(30):
c.tic()
alt = c.altitude_ft()
print(alt)
if alt > 200:
c.set_elevator(0.0)
c.ap_roll_hdg(180)
c.ap_pitch_vs(10)
c.toc(1.0)