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)