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