def process_messages(self, m):
mtype = m.get_type()
if mtype == "VFR_HUD":
self.label_airspeed.setText("AS: {:.1f}".format(m.airspeed))
self.label_gndspeed.setText("GS: {:.1f}".format(m.groundspeed))
self.label_heading.setText("Hdg: {:.0f}".format(m.heading))
self.label_throttle.setText("Thr {:.0%}".format(m.throttle))
self.label_altitude.setText("Alt: {:.0f}m".format(m.alt))
self.label_climb.setText("Clmb: {:.1f}".format(m.climb))
elif mtype == "ATTITUDE":
#self.label_roll.setText("Roll: {:.0f}".format(math.degrees(m.roll)))
#self.label_pitch.setText("Pitch: {:.0f}".format(math.degrees(m.pitch)))
self.horizon.roll_deg = math.degrees(m.roll)
self.horizon.pitch_deg = math.degrees(m.pitch)
self.horizon.update()
elif mtype == 'WIND':
self.label_wind.setText('Wind: %u/%.2f' % (m.direction, m.speed))
elif mtype == 'NAV_CONTROLLER_OUTPUT':
self.label_WPDist.setText('WP Dist %u' % m.wp_dist)
self.label_WPBearing.setText('Bearing %u' % m.target_bearing)
elif mtype in ['WAYPOINT_CURRENT', 'MISSION_CURRENT']:
self.label_WP.setText('WP %u' % m.seq)
elif mtype == 'HEARTBEAT':
flightmode = mavutil.mode_string_v10(m)
self.label_mode.setText("%s" % flightmode)
def process_messages(self, m):
mtype = m.get_type()
if mtype == "VFR_HUD":
self.label_airspeed.setText("AS: {:.1f}".format(m.airspeed))
self.label_gndspeed.setText("GS: {:.1f}".format(m.groundspeed))
self.label_heading.setText("Hdg: {:.0f}".format(m.heading))
self.label_throttle.setText("Thr {:.0%}".format(m.throttle))
self.label_altitude.setText("Alt: {:.0f}m".format(m.alt))
self.label_climb.setText("Clmb: {:.1f}".format(m.climb))
elif mtype == "ATTITUDE":
#self.label_roll.setText("Roll: {:.0f}".format(math.degrees(m.roll)))
#self.label_pitch.setText("Pitch: {:.0f}".format(math.degrees(m.pitch)))
self.horizon.roll_deg = math.degrees(m.roll)
self.horizon.pitch_deg = math.degrees(m.pitch)
self.horizon.update()
elif mtype == 'WIND':
self.label_wind.setText('Wind: %u/%.2f' % (m.direction, m.speed))
elif mtype == 'NAV_CONTROLLER_OUTPUT':
self.label_WPDist.setText('WP Dist %u' % m.wp_dist)
self.label_WPBearing.setText('Bearing %u' % m.target_bearing)
elif mtype in ['WAYPOINT_CURRENT', 'MISSION_CURRENT']:
self.label_WP.setText('WP %u' % m.seq)
elif mtype == 'HEARTBEAT':
flightmode = mavutil.mode_string_v10(m)
self.label_mode.setText("%s" % flightmode)
def read_mode(conn):
# reliable reading of flightmode
msg = None
while msg == None:
msg = conn.recv_match(type='HEARTBEAT', blocking=False)
# print('msg:',msg)
if msg:
mode = mavutil.mode_string_v10(msg)
return mode
def _heartbeatHandler(self, msg):
if not msg.autopilot == self._mavLib.MAV_AUTOPILOT_INVALID:
# Arm State
if msg.base_mode & self._mavLib.MAV_MODE_FLAG_SAFETY_ARMED != 0:
self._armed = True
else:
self._armed = False
# Mode
self._mode = mavutil.mode_string_v10(msg)
self.seenHeartbeat = True
def mainloop():
rospy.init_node('roscopter')
while not rospy.is_shutdown():
rospy.sleep(0.001)
msg = master.recv_match(blocking=False)
if not msg:
continue
#print msg.get_type()
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
else:
msg_type = msg.get_type()
'''
if msg_type == "RC_CHANNELS_RAW" :
pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw])
'''
if msg_type == "HEARTBEAT":
pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED,
mavutil.mode_string_v10(msg))
'''
if msg_type == "VFR_HUD":
pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb)
'''
if msg_type == "GPS_RAW_INT":
fix = NavSatStatus.STATUS_NO_FIX
if msg.fix_type >=3:
fix=NavSatStatus.STATUS_FIX
pub_gps.publish(NavSatFix(latitude = msg.lat/1e07,
longitude = msg.lon/1e07,
altitude = msg.alt/1e03,
status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS)
))
#pub.publish(String("MSG: %s"%msg))
if msg_type == "ATTITUDE" :
pub_attitude.publish(msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed)
if msg_type == "LOCAL_POSITION_NED" :
print "Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)
'''
def process(filename):
'''process one logfile'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename,
notimestamps=args.notimestamps,
robust_parsing=args.robust)
ext = os.path.splitext(filename)[1]
isbin = ext in ['.bin', '.BIN']
islog = ext in ['.log', '.LOG']
output = None
count = 1
dirname = os.path.dirname(filename)
if isbin or islog:
extension = "bin"
else:
extension = "tlog"
file_header = ''
messages = []
# we allow a list of modes that map to one mode number. This allows for --mode=AUTO,RTL and consider the RTL as part of AUTO
modes = args.mode.upper().split(',')
flightmode = None
while True:
m = mlog.recv_match()
if m is None:
break
if args.link is not None and m._link != args.link:
continue
mtype = m.get_type()
if mtype in messages:
if older_message(m, messages[mtype]):
continue
# we don't use mlog.flightmode as that can be wrong if we are extracting a single link
if mtype == 'HEARTBEAT' and m.get_srcComponent(
) != mavutil.mavlink.MAV_COMP_ID_GIMBAL and m.type != mavutil.mavlink.MAV_TYPE_GCS:
flightmode = mavutil.mode_string_v10(m).upper()
if mtype == 'MODE':
flightmode = mlog.flightmode
if (isbin or islog) and m.get_type() in ["FMT", "PARM", "CMD"]:
file_header += m.get_msgbuf()
if (isbin or islog
) and m.get_type() == 'MSG' and m.Message.startswith("Ardu"):
file_header += m.get_msgbuf()
if m.get_type() in ['PARAM_VALUE', 'MISSION_ITEM']:
timestamp = getattr(m, '_timestamp', None)
file_header += struct.pack('>Q',
timestamp * 1.0e6) + m.get_msgbuf()
if not mavutil.evaluate_condition(args.condition, mlog.messages):
continue
if flightmode in modes:
if output is None:
path = os.path.join(dirname,
"%s%u.%s" % (modes[0], count, extension))
count += 1
print("Creating %s" % path)
output = open(path, mode='wb')
output.write(file_header)
else:
if output is not None:
output.close()
output = None
if output and m.get_type() != 'BAD_DATA':
timestamp = getattr(m, '_timestamp', None)
if not isbin:
output.write(struct.pack('>Q', timestamp * 1.0e6))
output.write(m.get_msgbuf())
msg = data.recv_match();
size = int(math.ceil(app.root.winfo_height()/10)) #7.25
app.ARML.config(font=("Courier", size))
app.mode_label.config(font=("Courier", size))
app.mode_value.config(font=("Courier", size))
app.altitude_label.config(font=("Courier", size))
app.altitude_value.config(font=("Courier", size))
app.ground_speed_label.config(font=("Courier", size))
app.ground_speed_value.config(font=("Courier", size))
app.vertical_speed_label.config(font=("Courier", size))
app.vertical_speed_value.config(font=("Courier", size))
app.distance_to_next_waypoint_label.config(font=("Courier", size))
app.distance_to_next_waypoint_value.config(font=("Courier", size))
#If we have a valid message
if msg is not None:
if msg.get_type() is "HEARTBEAT":
app.mode.set(str(mavutil.mode_string_v10(msg)))
if '{0:08b}'.format(msg.base_mode)[0] is "0":
app.ARM.set("DISARMED")
app.ARML.config(foreground="black")
else:
app.ARM.set("ARMED")
app.ARML.config(foreground="red")
if msg.get_type() is "VFR_HUD":
app.altitude.set("{0:.2f} feet".format(msg.alt*3.28084)) #feet
app.ground_speed.set("{0:.2f} knots".format(msg.groundspeed*1.943844)) #knots
app.vertical_speed.set("{0:.2f} feet/min".format(msg.climb*196.85)) #feet/min
if msg.get_type() is "NAV_CONTROLLER_OUTPUT":
app.distance_to_next_waypoint.set("{0:.2f} nautical miles".format(msg.wp_dist*0.000539957)) #nautical miles/metres
def handle_heartbeat(msg):
mode = mavutil.mode_string_v10(msg)
is_armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
is_enabled = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED
def handle_heartbeat(self, msg):
#print("handle_heartbeat")
self.spabModel.mode = mavutil.mode_string_v10(msg)
self.spabModel.is_armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
self.spabModel.is_enabled = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED
def process(filename):
'''process one logfile'''
print("Processing %s" % filename)
mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps,
robust_parsing=args.robust)
ext = os.path.splitext(filename)[1]
isbin = ext in ['.bin', '.BIN']
islog = ext in ['.log', '.LOG']
output = None
count = 1
dirname = os.path.dirname(filename)
if isbin or islog:
extension = "bin"
else:
extension = "tlog"
file_header = ''
messages = []
# we allow a list of modes that map to one mode number. This allows for --mode=AUTO,RTL and consider the RTL as part of AUTO
modes = args.mode.upper().split(',')
flightmode = None
while True:
m = mlog.recv_match()
if m is None:
break
if args.link is not None and m._link != args.link:
continue
mtype = m.get_type()
if mtype in messages:
if older_message(m, messages[mtype]):
continue
# we don't use mlog.flightmode as that can be wrong if we are extracting a single link
if mtype == 'HEARTBEAT' and m.get_srcComponent() != mavutil.mavlink.MAV_COMP_ID_GIMBAL and m.type != mavutil.mavlink.MAV_TYPE_GCS:
flightmode = mavutil.mode_string_v10(m).upper()
if mtype == 'MODE':
flightmode = mlog.flightmode
if (isbin or islog) and m.get_type() in ["FMT", "PARM", "CMD"]:
file_header += m.get_msgbuf()
if (isbin or islog) and m.get_type() == 'MSG' and m.Message.startswith("Ardu"):
file_header += m.get_msgbuf()
if m.get_type() in ['PARAM_VALUE','MISSION_ITEM']:
timestamp = getattr(m, '_timestamp', None)
file_header += struct.pack('>Q', timestamp*1.0e6) + m.get_msgbuf()
if not mavutil.evaluate_condition(args.condition, mlog.messages):
continue
if flightmode in modes:
if output is None:
path = os.path.join(dirname, "%s%u.%s" % (modes[0], count, extension))
count += 1
print("Creating %s" % path)
output = open(path, mode='wb')
output.write(file_header)
else:
if output is not None:
output.close()
output = None
if output and m.get_type() != 'BAD_DATA':
timestamp = getattr(m, '_timestamp', None)
if not isbin:
output.write(struct.pack('>Q', timestamp*1.0e6))
output.write(m.get_msgbuf())
def run(self):
rospy.loginfo("Waiting for heartbeat")
try:
while not self.conn.wait_heartbeat(blocking=False) and not rospy.is_shutdown():
pass
if rospy.is_shutdown():
return
except SerialException as e:
# Ignore since we're shutting down
return
rospy.loginfo("Got heartbeat. Waiting 10secs for APM to be ready")
rospy.sleep(10)
self.conn.mav.request_data_stream_send(
self.conn.target_system,
self.conn.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL,
self.rate, 1)
# Send request to read waypoints
self.request_waypoints()
last_waypoint_read = time()
while not rospy.is_shutdown():
rospy.sleep(0.001)
msg = self.conn.recv_match(blocking=True)
if time() - self.last_waypoint_message_time > 5:
self.waypoint_read_in_progress = False
self.waypoint_write_in_progress = False
if time() - last_waypoint_read > 10:
last_waypoint_read = time()
self.request_waypoints()
if not msg:
continue
msg_type = msg.get_type()
if msg_type == "BAD_DATA":
rospy.logwarn("Got bad data")
continue
if self.export_conn:
self.export_conn.mav.send(msg)
if msg_type == "ATTITUDE":
self.pub_attitude.publish(
msg.roll, msg.pitch, msg.yaw,
msg.rollspeed, msg.pitchspeed, msg.yawspeed)
elif msg_type == "RC_CHANNELS_RAW":
self.pub_rcstate.publish([
msg.chan1_raw, msg.chan2_raw,
msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw,
msg.chan7_raw, msg.chan8_raw])
self.enable_control = msg.chan8_raw > 1700
if not self.enable_control:
self.enable_control_has_been_false = True
self.reset_rc_override()
elif msg_type == "RC_CHANNELS_SCALED":
pass
elif msg_type == "HEARTBEAT":
self.pub_basic_status.publish(
msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
mavutil.mode_string_v10(msg))
elif msg_type == "GPS_RAW_INT":
self.pub_gpsraw.publish(
msg.time_usec, msg.fix_type,
msg.lat / float(10 * 1000 * 1000),
msg.lon / float(10 * 1000 * 1000),
msg.alt / float(1000), msg.satellites_visible)
elif msg_type == "RAW_IMU":
self.pub_imuraw.publish(
msg.time_usec,
Vector3(msg.xgyro / 100.0, msg.ygyro / 100.0, msg.zgyro / 100.0),
Vector3(msg.xacc / 100.0, msg.yacc / 100.0, msg.zacc / 100.0),
Vector3(msg.xmag / 100.0, msg.ymag / 100.0, msg.zmag / 100.0))
elif msg_type == "MISSION_COUNT":
if not self.waypoint_read_in_progress:
rospy.logwarn("Did not expect MISSION_COUNT message")
else:
self.num_waypoints = msg.count
self.waypoint_buffer = []
# Ignore the first one, because it's some magic waypoint
if msg.count > 1:
# Request the first waypoint
self.last_waypoint_message_time = time()
self.conn.mav.mission_request_send(
self.conn.target_system,
self.conn.target_component,
1)
else:
self.waypoint_read_in_progress = False
elif msg_type == "MISSION_REQUEST":
if not self.waypoint_write_in_progress:
rospy.logwarn("Waypoint write not in progress, but received a request for a waypoint")
else:
waypoint = self.waypoint_buffer[msg.seq]
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
if msg.seq == 0:
# Waypoint zero seems to be special, and uses the
# GLOBAL frame. It also is magically reset in the
# firmware, so this probably doesn't matter.
frame = mavutil.mavlink.MAV_FRAME_GLOBAL
self.last_waypoint_message_time = time()
self.conn.mav.mission_item_send(
self.conn.target_system,
self.conn.target_component,
msg.seq,
frame,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
1 if msg.seq == 1 else 0, # Set current
1, # Auto continue after this waypoint
1.0, # "reached waypoint" is +/- 1.0m
5.0, # Stay for 5 secs then move on
1.0, # Stay within 1.0m for LOITER
0, # Face north on arrival
waypoint.latitude, # Latitude
waypoint.longitude, # Longitude
waypoint.altitude) # Altitude
elif msg_type == "MISSION_ACK":
if not self.waypoint_write_in_progress:
rospy.logwarn("Did not expect MISSION_ACK no write in progress")
# NOTE: APM is suppose to return MAV_CMD_ACK_OK, but it seems
# to return 0
elif msg.type not in (0, mavutil.mavlink.MAV_CMD_ACK_OK):
rospy.logerr("Bad MISSION_ACK: %d", msg.type)
self.waypoint_write_in_progress = False
else:
# All waypoints have been sent, read them back
self.waypoint_write_in_progress = False
self.last_waypoint_message_time = time()
self.conn.mav.mission_request_list_send(
self.conn.target_system,
self.conn.target_component)
self.waypoint_read_in_progress = True
elif msg_type == "MISSION_ITEM":
if not self.waypoint_read_in_progress:
rospy.logwarn("Did not expect MISSION_ITEM, no read in progress")
else:
self.waypoint_buffer.append(rospilot.msg.Waypoint(msg.x, msg.y, msg.z))
if self.num_waypoints == msg.seq + 1:
self.conn.mav.mission_ack_send(
self.conn.target_system,
self.conn.target_component,
mavutil.mavlink.MAV_CMD_ACK_OK)
self.pub_waypoints.publish(self.waypoint_buffer)
self.waypoint_read_in_progress = False
else:
self.last_waypoint_message_time = time()
self.conn.mav.mission_request_send(
self.conn.target_system,
self.conn.target_component,
msg.seq + 1)
import pymavlink.mavutil as mavutil
import time
device = ":20000"
mavproxy_conn = mavutil.mavlink_connection(device)
heartbeat_message = mavproxy_conn.recv_match (type = 'HEARTBEAT', blocking = True)
#while True:
# print(mavproxy_conn.recv_match (type = 'HEARTBEAT', blocking = True))
# print(mavproxy_conn.location())
# print(mavutil.mode_string_v10(heartbeat_message))
print(mavutil.mode_string_v10(heartbeat_message))
mavproxy_conn.set_mode_auto()
print(mavutil.mode_string_v10(mavproxy_conn.recv_match (type = 'HEARTBEAT', blocking = True)))
mavproxy_conn.set_mode('RTL')
print(mavutil.mode_string_v10(mavproxy_conn.recv_match (type = 'HEARTBEAT', blocking = True)))
#def device():
# return device
#def connection():
# return mavproxy_conn
def handle_heartbeat(msg):
mode = mavutil.mode_string_v10(msg)
is_armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
print("Heartbeat arming status is", is_armed)
is_enabled = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED
print("heartbeat enabled status is ", is_enabled)
