def wait_heartbeat(m):
'''wait for a heartbeat so we know the target system IDs'''
msg = m.recv_match(type='HEARTBEAT', blocking=True)
print("Heartbeat from APM (system %u component %u)" %
(m.target_system, m.target_system))
bytemsg = msg.get_msgbuf()
dmw.publish("mavlink", "opaque", bytemsg)
def process_messages(ac_id, msg):
'''show incoming pprz messages'''
global conv
global seq
global vehicle
global interface
if not isinstance(ac_id, str):
return
if "REQ" in msg.name:
ivystr = "gcs %s %s %s" % (str(ac_id), msg.name,
msg.payload_to_ivy_string())
ivystr = ivystr.strip()
dmw.publish("pprz", "opaque", ivystr)
def process_messages(c, m):
global aircraft_id
'''show incoming mavlink messages'''
while True:
msg = m.recv_match(blocking=True)
if not msg:
return
else:
if msg.get_type() == 'GLOBAL_POSITION_INT':
pos = Position()
pos.ecef_x, pos.ecef_y, pos.ecef_z = c.wgs2ecef(
float(msg.lat) / 1E7,
float(msg.lon) / 1E7,
float(msg.alt) / 1000.0)
pos.lat, pos.lon, pos.alt = float(msg.lat) / 1E7, float(
msg.lon) / 1E7, float(msg.alt) / 1000.0
pos.vehicle_id = aircraft_id
pos.vehicle_tag = str(aircraft_id)
pos.time_boot_ms = msg.time_boot_ms
pos.relalt = float(msg.relative_alt) / 1000.0
pos.vx = float(msg.vx) / 100.0
pos.vy = float(msg.vy) / 100.0
pos.vz = float(msg.vz) / 100.0
pos.vground = math.sqrt(pos.vx * pos.vx + pos.vy * pos.vy)
pos.hdg = float(msg.hdg) / 100.0
pos.seq = msg.get_seq()
dmw.publish("vehicle", "position", pos.SerializeToString())
dmw.store("vehicle", "position", str(pos.vehicle_id),
pos.SerializeToString())
elif msg.get_type() == 'HEARTBEAT':
hb = HeartBeat()
hb.uavtype = find_uav_type(msg.type)
hb.autopilot = find_ap_type(msg.autopilot)
hb.vehicle_id = aircraft_id
hb.vehicle_tag = str(aircraft_id)
hb.base_mode = msg.base_mode
hb.custom_mode = msg.custom_mode
hb.system_status = msg.system_status
hb.mavlink_version = msg.mavlink_version
hb.seq = msg.get_seq()
dmw.publish("vehicle", "heartbeat", hb.SerializeToString())
dmw.store_set("vehicle",
"heartbeat",
str(hb.vehicle_id),
hb.SerializeToString(),
expire=60)
else:
dmw.publish("mavlink", "opaque", msg.get_msgbuf())
def process_messages(c,m):
global aircraft_id
'''show incoming mavlink messages'''
while True:
msg = m.recv_match(blocking=True)
if not msg:
return
else:
if msg.get_type() == 'GLOBAL_POSITION_INT':
pos = Position()
pos.ecef_x, pos.ecef_y, pos.ecef_z = c.wgs2ecef( float(msg.lat) / 1E7, float(msg.lon) / 1E7, float(msg.alt) / 1000.0 )
pos.lat, pos.lon, pos.alt = float(msg.lat) / 1E7, float(msg.lon) / 1E7, float(msg.alt) / 1000.0
pos.vehicle_id = aircraft_id
pos.vehicle_tag = str(aircraft_id)
pos.time_boot_ms = msg.time_boot_ms;
pos.relalt = float( msg.relative_alt ) / 1000.0
pos.vx = float(msg.vx) / 100.0;
pos.vy = float(msg.vy) / 100.0;
pos.vz = float(msg.vz) / 100.0;
pos.vground = math.sqrt( pos.vx*pos.vx + pos.vy*pos.vy )
pos.hdg = float( msg.hdg ) / 100.0
pos.seq = msg.get_seq()
dmw.publish( "vehicle", "position", pos.SerializeToString() )
dmw.store( "vehicle", "position", str(pos.vehicle_id), pos.SerializeToString() )
elif msg.get_type() == 'HEARTBEAT':
hb = HeartBeat()
hb.uavtype = find_uav_type( msg.type )
hb.autopilot = find_ap_type( msg.autopilot )
hb.vehicle_id = aircraft_id
hb.vehicle_tag = str( aircraft_id )
hb.base_mode = msg.base_mode
hb.custom_mode = msg.custom_mode
hb.system_status = msg.system_status
hb.mavlink_version = msg.mavlink_version
hb.seq = msg.get_seq()
dmw.publish( "vehicle", "heartbeat", hb.SerializeToString() )
dmw.store_set( "vehicle", "heartbeat", str(hb.vehicle_id), hb.SerializeToString(), expire=60 )
else:
dmw.publish( "mavlink", "opaque", msg.get_msgbuf() )
def on_config_req_msg(self, agent, *larg):
dmw.publish("pprz", "opaque",
"%s %s CONFIG_REQ %s" % (larg[0], larg[1], larg[2]))
def on_aircraft_req_msg(self, agent, *larg):
dmw.publish("pprz", "opaque", "%s %s AIRCRAFTS_REQ" % (larg[0], larg[1]))
def process_messages( ac_id, msg ):
'''show incoming pprz messages'''
global conv
global seq
global vehicle
if ac_id != 0:
return
if msg.name == 'AP_STATUS':
vehicle.ap_mode = ap_modes[ msg.ap_mode ]
vehicle.gps_fix = gps_modes[ msg.gps_mode ]
if msg.kill_mode == "ON":
vehicle.killed = True
else:
vehicle.killed = False
vehicle.flight_time = msg.flight_time
# <field name="state_filter_mode" type="string" values="UNKNOWN|INIT|ALIGN|OK|GPS_LOST|IMU_LOST|COV_ERR|IR_CONTRAST|ERROR"/>
elif msg.name == 'FLIGHT_PARAM':
vehicle.heading = float(msg.heading)
vehicle.lat = float(msg.lat)
vehicle.lon = float(msg.long)
vehicle.alt = float(msg.alt)
vehicle.relalt = float(msg.agl)
vehicle.unix_time = int(float(msg.unix_time))
vehicle.course = float(msg.course)
vehicle.sog = float(msg.speed)
vehicle.airspeed = float(msg.airspeed)
crsrad = (math.pi/180.0) * vehicle.course
vehicle.vx = vehicle.sog * math.cos( crsrad )
vehicle.vy = vehicle.sog * math.sin( crsrad )
vehicle.vz = float(msg.climb)
pos = Position()
pos.ecef_x, pos.ecef_y, pos.ecef_z = conv.wgs2ecef( vehicle.lat, vehicle.lon, vehicle.alt )
pos.lat, pos.lon, pos.alt = vehicle.lat, vehicle.lon, vehicle.alt
pos.vehicle_id = vehicle.ac_id
pos.vehicle_tag = str(vehicle.ac_id)
pos.time_boot_ms = vehicle.unix_time;
pos.relalt = vehicle.relalt
pos.vx = vehicle.vx
pos.vy = vehicle.vy
pos.vz = vehicle.vz
pos.vground = vehicle.sog
pos.hdg = vehicle.heading
pos.seq = update_seq()
dmw.publish( "vehicle", "position", pos.SerializeToString() )
dmw.store( "vehicle", "position", str(pos.vehicle_id), pos.SerializeToString() )
# <field name="roll" type="float" unit="deg"/>
# <field name="pitch" type="float" unit="deg"/>
# <field name="itow" type="uint32" unit="ms"/>
elif msg.name == 'TELEMETRY_STATUS':
hb = HeartBeat()
hb.uavtype = "GENERIC"
hb.autopilot = "PPZ"
hb.vehicle_id = vehicle.ac_id
hb.vehicle_tag = str(vehicle.ac_id)
hb.base_mode = vehicle.ap_mode
hb.custom_mode = 0
hb.system_status = mavutil.mavlink.MAV_STATE_ACTIVE
hb.mavlink_version = 3
hb.seq = update_seq()
dmw.publish( "vehicle", "heartbeat", hb.SerializeToString() )
dmw.store_set( "vehicle", "heartbeat", str(hb.vehicle_id), hb.SerializeToString(), expire=60 )
else:
dmw.publish( "pprz", "opaque", "%s %s %s"%( msg.msg_class, msg.name, msg.payload_to_ivy_string() ) )
def on_config_msg( self, agent, *larg ):
dmw.publish( "pprz", "opaque", "%s %s CONFIG %s"%( larg[0], larg[1], larg[2] ) )
def on_aircrafts_msg( self, agent, *larg ):
dmw.publish( "pprz", "opaque", "%s %s AIRCRAFTS %s"%( larg[0], larg[1], larg[2] ) )
def print_rcvd_message(msg_class, msg_name, sender, message):
print "%s.%s.%s: %s" % (msg_class, msg_name, sender, message)
# called by each thread
class SubscriptionThread(threading.Thread):
def run(self):
print "initializing sub, subscribing and running"
dmw.init_sub()
dmw.subscribe("telemetry", "position", "", print_rcvd_message)
dmw.loop()
print "exited run"
if __name__ == "__main__":
dmw.init()
dmw.init_pub("testinpython")
# Subscribe first
t = SubscriptionThread()
t.setDaemon(True)
t.start()
time.sleep(1)
dmw.publish("telemetry", "position", "14.1525354 102.23324324 25.3335")
time.sleep(3)
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(('127.0.0.1', 5400 ))
s.listen(1)
while True:
conn, addr = s.accept()
# Subscribe to messages from gcs.
t = SubscriptionThread( conn, aircraft_id )
t.setDaemon( True )
t.start()
print 'Connected by', addr
try:
while True:
data = conn.recv(1024)
if not data:
continue
else:
dmw.publish( "mavlink", "opaque", data )
except socket.error as e:
print("Client disconnected. Accepting another.")
t.cancel()
print ("Exiting app")
import threading
import time
def print_rcvd_message( msg_class, msg_name, sender, message ):
print "%s.%s.%s: %s"%( msg_class, msg_name, sender, message)
# called by each thread
class SubscriptionThread(threading.Thread):
def run(self):
print "initializing sub, subscribing and running"
dmw.init_sub()
dmw.subscribe( "telemetry", "position", "", print_rcvd_message )
dmw.loop()
print "exited run"
if __name__ == "__main__":
dmw.init()
dmw.init_pub( "testinpython" )
# Subscribe first
t = SubscriptionThread()
t.setDaemon( True )
t.start()
time.sleep(1)
dmw.publish( "telemetry", "position", "14.1525354 102.23324324 25.3335" )
time.sleep(3)
def wait_heartbeat(m):
'''wait for a heartbeat so we know the target system IDs'''
msg = m.recv_match(type='HEARTBEAT', blocking=True)
print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system))
bytemsg = msg.get_msgbuf()
dmw.publish( "mavlink", "opaque", bytemsg )