"""
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
from fgclient import FgClient
c = FgClient()
a_des = c.altitude_ft() + 100.0
int_err = 0.0
kk = 0
while True:
kk += 1
c.tic()
a = c.altitude_ft()
vs_des = 0.05 * (a_des - a)
vs = c.vertical_speed_fps()
err = (vs_des - vs)
int_err += 0.5 * err
c.set_elevator(-0.04 * err - 0.02 * int_err)
print(a_des, a, vs_des, vs)
c.toc(0.5)
from fgclient import FgClient
c = FgClient()
c.ap_pitch_off()
from math import sin
kk = 0
while True:
kk += 1
c.tic()
vs = c.vertical_speed_fps()
print(vs)
c.set_elevator(0.75*sin(0.5*kk))
c.toc(0.5)
"""
Execute step input on elevator and record step response in log
"""
from fgclient import FgClient
c = FgClient()
# require level flight to begin
vs = c.vertical_speed_fps()
if abs(vs) > 0.2:
raise ValueError('Vertical speed too large: ', vs)
# and require zero elevator
el = c.get_elevator()
if el != 0.0:
raise ValueError('Elevator not central: ', el)
c.ap_pitch_off()
time_step = 0.5
for kk in range(120):
c.tic()
vs = c.vertical_speed_fps()
if kk < 10:
el = 0.0
else:
el = -0.02
c.set_elevator(el)
print(kk, el, vs)
c.toc(0.5)
c.set_elevator(0.0)
c.ap_pitch_vs(0.0)
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)
"""
Initial skeleton example: run a proportional controller
on the vertical speed, and look at a step response
"""
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()
kk = 0
dt = 0.5
while True:
kk += 1
c.tic()
if kk > 10:
vs_des = 5.0
else:
vs_des = 0.0
vs = c.vertical_speed_fps()
c.set_elevator(-0.01 * (vs_des - vs))
print(vs)
c.toc(dt)
"""
Implement PID control on the heading of FlightGear simulated aircraft
"""
from fgclient import FgClient
c = FgClient()
c.ap_roll_off()
kk = 0
dt = 0.5
integral_error = 0.0
initial_hdg = c.heading_deg()
last_hdg = initial_hdg
while True:
kk += 1
c.tic()
if kk > 10:
hdg_des = initial_hdg + 15
else:
hdg_des = initial_hdg
hdg = c.heading_deg()
# differentiate
hdg_deriv = (hdg - last_hdg) / dt
last_hdg = hdg
# integrate
integral_error += dt * (hdg_des - hdg)
c.set_aileron(0.01 *
((hdg_des - hdg) - 4 * hdg_deriv + 0.03 * integral_error))
print(hdg_des, hdg)
c.toc(dt)
from fgclient import FgClient
c = FgClient()
c.ap_roll_off()
kk = 0
dt = 0.5
initial_hdg = c.heading_deg()
while True:
kk += 1
c.tic()
if kk > 10:
hdg_des = initial_hdg+15
else:
hdg_des = initial_hdg
hdg = c.heading_deg()
c.set_aileron(0.01*(hdg_des - hdg))
print(hdg_des,hdg)
c.toc(dt)
from fgclient import FgClient c = FgClient() c.ap_pitch_off() while True: c.tic() vs = c.vertical_speed_fps() e = c.get_elevator() print(e,vs) c.toc(0.5)
from fgclient import FgClient
c = FgClient()
c.ap_pitch_vs()
c.ap_roll_off()
kk = 0
error = 0
err_sum = 0
kp = 0.005
ki = 0
kd = 0.05
dt = 0.5
first_hdg = c.heading_deg()
last_hdg = first_hdg
while True:
kk += 1
c.tic()
if kk > 1:
head_des = first_hdg + 15
else:
head_des = first_hdg
head = c.heading_deg()
error = head_des - head
err_sum += error
err_deriv = (head - last_hdg) / dt
last_hdg = head
c.set_aileron(kp * (error) - ki * err_sum - kd * err_deriv)
print(head)
c.toc(dt)
from fgclient import FgClient
c = FgClient()
c.ap_pitch_off()
initial_altitide = c.altitude_ft()
integral_error = 0.0
kk = 0
dt = 0.5
while True:
kk += 1
c.tic()
if kk > 10:
alt_des = initial_altitide + 100
else:
alt_des = initial_altitide
alt = c.altitude_ft()
vs_des = 0.03*(alt_des - alt)
vs = c.vertical_speed_fps()
integral_error = integral_error + dt*(vs_des - vs)
c.set_elevator(-0.014*((vs_des - vs) + 0.4*integral_error))
print(alt_des,alt,vs_des,vs)
c.toc(dt)
from fgclient import FgClient c = FgClient(savelog=False) # centre controls c.set_elevator(0.0) c.set_aileron(0.0) # AP on level and south c.ap_pitch_vs() c.ap_roll_hdg(180.0)
from fgclient import FgClient
c = FgClient()
c.ap_pitch_off()
int_err = 0.0
kk = 0
while True:
kk+=1
c.tic()
if kk>10:
vs_des = 5.0
else:
vs_des = 0.0
vs = c.vertical_speed_fps()
err = (vs_des - vs)
int_err += 0.5*err
c.set_elevator(-0.04*err - 0.02*int_err)
print(vs)
c.toc(0.5)
from fgclient import FgClient
c = FgClient()
c.ap_pitch_off()
kk = 0
error = 0
err_sum = 0
while True:
kk += 1
c.tic()
if kk > 10:
vs_des = 5.0
else:
vs_des = 0.0
vs = c.vertical_speed_fps()
error = vs_des - vs
err_sum += error
c.set_elevator(-0.03 * (error) - 0.005 * err_sum)
print(vs)
c.toc(0.5)
"""
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)