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()
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)
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)