def mavlink_message_handler(msg):
global mavlink_recv
if msg.msgid == 126:
mav_bytes_msg = mavlink.convert_to_bytes(msg)
mav_msg = link.decode(mav_bytes_msg)
mavlink_recv += ''.join(chr(x) for x in mav_msg.data[:mav_msg.count])
if 'nsh>' in mavlink_recv:
# Remove the last line, including newline before prompt
mavlink_recv = mavlink_recv[:mavlink_recv.find('nsh>') - 1]
recv_event.set()
def mavlink_message_handler(msg):
global param1, param2
if msg.msgid == 76:
mav_bytes_msg = mavlink.convert_to_bytes(msg)
mav_msg = link.decode(mav_bytes_msg)
if mav_msg.command == 31011:
print(mav_msg)
if mav_msg.param1 != param1 and mav_msg.param2 != param2:
run_tracker(640 * mav_msg.param1, 480 * mav_msg.param2)
param1 = mav_msg.param1
param2 = mav_msg.param2
def time_callback(data):
"""
Synchronize time of messages we send with the FCU. Extracts the FCU
time from a TIMESYNC mavlink message, and uses this to set global variables
"""
global secs
global nsecs
b = convert_to_bytes(data)
m = mav.decode(b)
if (m.get_msgId() == 111): # timesync message
s = m.tc1 / 1.0e9
t = rospy.Time.from_sec(s)
secs = t.secs
nsecs = t.nsecs
def mavlink_message_handler(msg):
global gupdate
global mavlink_recv
global ht_time
global logauto
if ht_time + 4 < time.time():
mavlink_pub.publish(ros_msg)
ht_time = time.time()
if msg.msgid == 126 or msg.msgid == 120 or msg.msgid == 253:
mav_bytes_msg = mavlink.convert_to_bytes(msg)
mav_msg = link.decode(mav_bytes_msg)
if hasattr(mav_msg, 'data'):
mavlink_recv += ''.join(
chr(x) for x in mav_msg.data[:mav_msg.count])
if 'nsh>' in mavlink_recv:
mvr = mavlink_recv.replace("\x1b[K", "").split("\n")
if "nsh> " in mvr:
mvr.remove("nsh> ")
mavlink_recv = ''
print mvr
if hasattr(gupdate,
'message') and str.join("", mvr).strip() != '':
gupdate.message.reply_text(str.join("\n", mvr))
recv_event.set()
if hasattr(mav_msg, 'text'):
if logauto != None and msg.msgid == 253 and 'ARMED' == mav_msg.text[:
5]:
startLog("", gupdate)
if logauto != None and msg.msgid == 253 and 'DISARMED' == mav_msg.text[:
8]:
stopLog("", gupdate)
if hasattr(gupdate, 'message') and hasattr(mav_msg, 'text'):
gupdate.message.reply_text(mav_msg.text)
print mav_msg
def mavlink_message_handler(msg):
print link.decode(mavlink.convert_to_bytes(msg))