def sim_send(m, a):
sim_send_UT(m, a)
'''send flight information to mavproxy and flightgear'''
global fdm
from math import degrees
earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
(roll, pitch, yaw) = a.dcm.to_euler()
fdm.set('latitude', a.latitude, units='degrees')
fdm.set('longitude', a.longitude, units='degrees')
fdm.set('altitude', a.altitude, units='meters')
fdm.set('phi', roll, units='radians')
fdm.set('theta', pitch, units='radians')
fdm.set('psi', yaw, units='radians')
fdm.set('phidot', earth_rates.x, units='rps')
fdm.set('thetadot', earth_rates.y, units='rps')
fdm.set('psidot', earth_rates.z, units='rps')
fdm.set('vcas',
math.sqrt(a.velocity.x * a.velocity.x +
a.velocity.y * a.velocity.y),
units='mps')
fdm.set('v_north', a.velocity.x, units='mps')
fdm.set('v_east', a.velocity.y, units='mps')
# FG FDM protocol only supports 4 motors for display :(
fdm.set('num_engines', 4)
for i in range(4):
fdm.set('rpm', 1000 * m[i], idx=i)
try:
fg_out.send(fdm.pack())
except socket.error as e:
if not e.errno in [errno.ECONNREFUSED]:
raise
timestamp = int((a.time_now - a.time_base) * 1e6)
buf = struct.pack(
'<Q17dI', timestamp, a.latitude, a.longitude, a.altitude, degrees(yaw),
a.velocity.x, a.velocity.y, a.velocity.z, a.accelerometer.x,
a.accelerometer.y, a.accelerometer.z, degrees(earth_rates.x),
degrees(earth_rates.y), degrees(earth_rates.z), degrees(roll),
degrees(pitch), degrees(yaw),
math.sqrt(a.velocity.x * a.velocity.x + a.velocity.y * a.velocity.y),
0x4c56414f)
try:
sim_out.send(buf)
except socket.error as e:
if not e.errno in [errno.ECONNREFUSED]:
raise
def sim_send_UT(m, a):
global i
i+=1
if i%40 == 0:
earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
(roll, pitch, yaw) = a.dcm.to_euler()
yaw = np.rad2deg(yaw)
roll = np.rad2deg(roll)
pitch = np.rad2deg(pitch)
utm_quad_pos = utm.from_latlon(a.latitude,a.longitude)
print utm_quad_pos[0],utm_quad_pos[1],a.altitude,roll,pitch,yaw, a.velocity.x, a.velocity.y, a.velocity.z
dataToSend = struct.pack("ddddddddd",utm_quad_pos[0],utm_quad_pos[1],a.altitude,roll,pitch,yaw,a.velocity.x, a.velocity.y, a.velocity.z)
ut_out.sendto(dataToSend,ut_out_address)
def sim_send(a):
'''send flight information to mavproxy'''
from math import degrees
earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro)
(roll, pitch, yaw) = a.dcm.to_euler()
buf = struct.pack(
'<16dI', a.latitude, a.longitude, a.altitude, degrees(yaw),
a.velocity.x, a.velocity.y, a.accelerometer.x, a.accelerometer.y,
a.accelerometer.z, degrees(earth_rates.x), degrees(earth_rates.y),
degrees(earth_rates.z), degrees(roll), degrees(pitch), degrees(yaw),
math.sqrt(a.velocity.x * a.velocity.x + a.velocity.y * a.velocity.y),
0x4c56414e)
try:
sim_out.send(buf)
except socket.error as e:
if not e.errno in [errno.ECONNREFUSED]:
raise
