def task_zero_gyros(self):
print("request reset ekf")
self.update()
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
self.send_value(t, "/sensors/Aura3/command", "zero_gyros")
t.quit()
def update(self):
print "update parameters"
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
#self.send_value(t, "/task/preflight/duration_sec",
# self.edit_duration.text())
t.quit()
def update(self):
print("update land params")
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
self.send_value(t, "/task/land/lateral_offset_m",
self.edit_lat_offset.text())
self.send_value(t, "/task/land/glideslope_deg",
self.edit_glideslope.text())
self.send_value(t, "/task/land/turn_radius_m",
self.edit_turn_radius.text())
self.send_value(t, "/task/land/direction",
self.edit_direction.currentText())
self.send_value(t, "/task/land/extend_final_leg_m",
self.edit_final_leg.text())
self.send_value(t, "/task/land/altitude_bias_ft",
self.edit_alt_bias.text())
self.send_value(t, "/task/land/approach_speed_kt",
self.edit_appr_speed.text())
self.send_value(t, "/task/land/flare_pitch_deg",
self.edit_flare_pitch.text())
self.send_value(t, "/task/land/flare_seconds",
self.edit_flare_time.text())
# send these last so our 'route' doesn't stay moved long
# before we kick into land mode.
if len(self.edit_rwy_lon.text()):
self.send_value(t, "/task/home/longitude_deg",
self.edit_rwy_lon.text())
if len(self.edit_rwy_lat.text()):
self.send_value(t, "/task/home/latitude_deg",
self.edit_rwy_lat.text())
if len(self.edit_rwy_hdg.text()):
self.send_value(t, "/task/home/azimuth_deg",
self.edit_rwy_hdg.text())
t.quit()
def revert(self):
# revert form
if self.type == "pid":
self.edit_Kp.setText(self.original_values[0])
self.edit_Ti.setText(self.original_values[1])
self.edit_Td.setText(self.original_values[2])
self.edit_min.setText(self.original_values[3])
self.edit_max.setText(self.original_values[4])
self.edit_trim.setText(self.original_values[5])
elif self.type == "vel":
self.edit_Kp.setText(self.original_values[0])
self.edit_beta.setText(self.original_values[1])
self.edit_alpha.setText(self.original_values[2])
self.edit_gamma.setText(self.original_values[3])
self.edit_Ti.setText(self.original_values[4])
self.edit_Td.setText(self.original_values[5])
self.edit_min.setText(self.original_values[6])
self.edit_max.setText(self.original_values[7])
self.edit_trim.setText(self.original_values[8])
# send original values to remote
command = "fcs-update " + str(self.index)
for value in self.original_values:
command += "," + value
#print "update: " + str(self.value_array())
print command
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
t.send(command)
t.quit()
def task_reset_ekf(self):
print("request reset ekf")
self.update()
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
self.send_value(t, "/filters/command", "reset")
t.quit()
def task_resume(self):
print "Resume route ..."
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port == 5402:
t.send("send task,resume")
else:
t.send("set /task/command-request task,resume")
t.quit()
def task_resume(self):
print("Resume route ...")
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port != 6499:
t.send("send task,resume")
else:
t.send("set /task/command resume")
t.quit()
def task_home(self):
print("request circle hold at home")
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port != 6499:
t.send("send task,home")
else:
t.send("set /task/command home")
t.quit()
def task_circle(self):
print("request circle hold at current location")
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port != 6499:
t.send("send task,circle")
else:
t.send("set /task/command circle")
t.quit()
def task_calibrate(self):
print "request calibreate task"
self.update()
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port != 6499:
t.send("send task,calibrate")
else:
t.send("set /task/command_request task,calibrate")
t.quit()
def task_gofly(self):
print("request parametric path")
self.update()
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port != 6499:
t.send("send task,parametric")
else:
t.send("set /task/command task,parametric")
t.quit()
def task_preflight(self):
print("request preflight mode")
self.update()
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port != 6499:
t.send("send task,preflight")
else:
t.send("set /task/command task,preflight")
t.quit()
def update(self):
print "update L1 params"
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
self.send_value(t, "/config/autopilot/L1_controller/bank_limit_deg",
self.edit_bank_limit.text())
self.send_value(t, "/config/autopilot/L1_controller/period",
self.edit_L1_period.text())
self.send_value(t, "/config/autopilot/L1_controller/damping",
self.edit_L1_damping.text())
t.quit()
def update(self):
command = "fcs-update " + str(self.index)
for value in self.value_array():
command += "," + value
# print "update: " + str(self.value_array())
print command
print self.port
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
t.send(command)
t.quit()
def update(self):
print("update circle hold params")
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
self.send_value(t, "/autopilot/targets/altitude_agl_ft",
self.edit_alt.text())
self.send_value(t, "/autopilot/targets/airspeed_kt",
self.edit_speed.text())
self.send_value(t, "/task/circle/direction",
self.edit_direction.currentText())
self.send_value(t, "/task/circle/radius_m", self.edit_radius.text())
t.quit()
def task_launch(self):
print("Launch!")
# send over current launching configuration
self.update()
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
if self.port != 6499:
t.send("send task,launch")
else:
t.send("set /task/command launch")
t.quit()
def update(self):
print "update L1 params"
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
self.send_value(t, "/config/autopilot/TECS/mass_deg",
self.edit_mass.text())
self.send_value(t, "/config/autopilot/TECS/weight_tot",
self.edit_weight_tot.text())
self.send_value(t, "/config/autopilot/TECS/weight_bal",
self.edit_weight_bal.text())
self.send_value(t, "/config/autopilot/TECS/min_kt",
self.edit_min_kt.text())
self.send_value(t, "/config/autopilot/TECS/max_kt",
self.edit_max_kt.text())
t.quit()
def update(self):
print("update launch params")
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
self.send_value(t, "/task/launch/speed_kt", self.edit_speed.text())
self.send_value(t, "/task/launch/completion_agl_ft",
self.edit_completion_agl.text())
self.send_value(t, "/task/launch/mission_agl_ft",
self.edit_mission_agl.text())
self.send_value(t, "/task/launch/roll_gain",
self.edit_roll_gain.text())
self.send_value(t, "/task/launch/roll_limit",
self.edit_roll_limit.text())
self.send_value(t, "/task/launch/rudder_enable",
self.edit_rudder_enable.currentText())
self.send_value(t, "/task/launch/rudder_gain",
self.edit_rudder_gain.text())
self.send_value(t, "/task/launch/rudder_max", self.rudder_max.text())
t.quit()
def task_land(self):
print("Land!")
# send over current landing configuration and touchdown point
self.update()
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
cmd = "task,land"
az = self.edit_rwy_hdg.text()
if len(az):
cmd += "," + az
if self.port != 6499:
cmd = 'send ' + cmd
else:
cmd = 'set /task/command ' + cmd
print('sending:', cmd)
t.send(str(cmd))
t.quit()
def update(self):
print "update circle hold params"
t = fgtelnet.FGTelnet(self.host, self.port)
t.send("data")
start_str = self.edit_start_freq.text()
if start_str == "": start_str = "0.0"
freq_start = "%.2f" % (float(start_str) * 2.0 * math.pi)
self.send_value(t, "/task/chirp/freq_start_rad_sec", freq_start)
end_str = self.edit_end_freq.text()
if end_str == "": end_str = "0.0"
freq_end = "%.2f" % (float(end_str) * 2.0 * math.pi)
self.send_value(t, "/task/chirp/freq_end_rad_sec", freq_end)
self.send_value(t, "/task/chirp/duration_sec",
self.edit_duration.text())
ampl_str = self.edit_ampl.text()
if ampl_str == "": ampl_str = "0.0"
ampl = "%.3f" % (float(ampl_str) * math.pi / 180.0)
self.send_value(t, "/task/chirp/amplitude", ampl)
self.send_value(t, "/controls/signal/inject",
self.edit_inject.currentText())
t.quit()
def connect(self, host="localhost", port=6499):
self.t = fgtelnet.FGTelnet(host, port)
self.t.send("data")
from collections import deque
import time
import fgtelnet
hz = 10
dt = 1.0 / float(hz)
seconds = 100
lines = deque()
#port = 6499
port = 5402
t = fgtelnet.FGTelnet("localhost", port)
t.send("data")
count = 1
while True:
t.send("fcs all")
result = t.receive()
tokens = map(float, result.split(','))
if tokens[3] < 0.0:
tokens[3] += 360.0
print str(tokens[3])
tokens[5] *= 25.0
tokens[11] *= 25.0
tokens[15] *= 200.0
line = " ".join(map(str, tokens))
print line
lines.append(line)
print "x_sum = %.2f y_sum = %.2f counter = %d" % (x_sum, y_sum, counter)
# zero accumulators
x_sum = 0.0
y_sum = 0.0
counter = 0
avg_speed = math.sqrt( x*x + y*y )
if avg_speed > 0.5:
# Only update track if speed is above a threshold of movement.
# This way we don't get random orientations if the base station
# isn't moving. Note: 0.5 mps =~ 1 kt
track = 90.0 - math.atan2( y, x ) / deg2rad
if track < 0:
track += 360.0
print "Time = %.1f avg track = %.1f x = %.2f y = %.2f spd = %.2f" % (cur_time, track, x, y, avg_speed)
message = "send home,%.8f,%.8f,0,%.1f" % \
( gpsd.fix.longitude, gpsd.fix.latitude, track)
print message
try:
t = fgtelnet.FGTelnet(host, port)
t.send("data")
t.send(message)
t.quit()
except:
print "Cannot connect to uglink"
except StopIteration:
print "GPSD has terminated"
