def __init__(self, shortHand, mavSystemID, mavComponentID, noRWSleepTime=0.1, loopPauseSleepTime=0.5): self._name = shortHand self.loopPauseSleepTime = loopPauseSleepTime self.noRWSleepTime = noRWSleepTime self._mavSystemID = mavSystemID self._mavComponentID = mavComponentID self._writeQueue = queue.Queue() self._loopRunning = False self._intentionallyExit = False if self._ser is None or not isinstance(self._ser, commAbstract): raise NotImplementedError('_ser must be set to a object ' 'inheriting from `commAbstract` before ' 'super constructor is called') try: self._mavObj = pymavlink.MAVLink(file=self._ser, srcSystem=mavSystemID, srcComponent=mavComponentID) except: raise Exception('Unable to create mavlink object') # Add reference to port dictionary MAVAbstract.portDict[self._name] = self
def pozyx_positioning_pub(r): mavlink_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=5) rospy.init_node('positioning_pub', anonymous=True) #Subscribe to get altitude rospy.Subscriber("/mavros/imu/static_pressure", FluidPressure, vfr_callback) #Subscribe to get attitude rospy.Subscriber("/mavros/imu/data", Imu, imu_data_callback) f = filo() mav = MAV_APM.MAVLink(f, srcSystem=1, srcComponent=1) rospy.sleep(1) # let data show up in subs pose = types.SimpleNamespace() while mavlink_pub.get_num_connections() <= 0: pass try: r.loop() except: rospy.loginfo("Pozyx not connected") return while not rospy.is_shutdown(): coords = pypozyx.Coordinates() pozyx.doPositioning(coords, remote_id=remote_id) r = R.from_quat([ imu_data.orientation.w, imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z]) attitude_rad = r.as_euler('xyz', degrees=False) pose.yaw = attitude_rad[0] pose.yaw += np.pi*3/2 if pose.yaw>np.pi: pose.yaw -=2*np.pi pose.pitch = attitude_rad[1] pose.roll = attitude_rad[2] if not (coords.x == 0 and coords.y==0): temp_x = coords.x/1000 temp_y = coords.y/1000 pose.e = (temp_x*np.cos(3.7)+temp_y*np.sin(3.7)) pose.n = -(-temp_x*np.cos(3.7)+temp_y*np.sin(3.7)) pose.z = -altitude # print("n: ", pose.n, "e: ", pose.e, "z: ", pose.z, "yaw: ", pose.yaw) send_vision_position_estimate(mav, mavlink_pub, pose)
def set_global_origin(self): """ Send a mavlink SET_GPS_GLOBAL_ORIGIN message, which allows us to use local position information without a GPS. """ target_system = 0 # target_system = 0 # 0 --> broadcast to everyone lattitude = 30 longitude = 130 altitude = 1 f = fifo() mav = MAV_APM.MAVLink(f, srcSystem=1, srcComponent=1) msg = MAV_APM.MAVLink_set_gps_global_origin_message( target_system, lattitude, longitude, altitude) msg.pack(mav) self.vehicle.send_mavlink(msg) self.vehicle.flush()
def set_home(lat, lon, alt, x, y, z): try: mavlink_pub = rospy.Publisher("/mavlink/to", Mavlink, queue_size=20) # Set up mavlink instance f = fifo() mav = MAV_APM.MAVLink(f, srcSystem=1, srcComponent=1) # wait to initialize while mavlink_pub.get_num_connections() <= 0: pass for _ in range(5): rospy.sleep(1) set_global_origin(mav, mavlink_pub, lat, lon, alt) set_home_position(mav, mavlink_pub, lat, lon, alt, x, y, z) except rospy.ROSInterruptException: pass
# Buffer classe to write and read, necessary to start the MAVLink dialect class fifo(object): def __init__(self): self.buf = [] def write(self, data): self.buf += data return len(data) def read(self): return self.buf.pop(0) # Creating one buffer class f = fifo() mav = mavlink1.MAVLink( f) # Dialeto a ser usado, todas as mensagens e tipos aqui # Aqui ja testando a mensagemn a ser utilizada para enviar os valores de setpoint roll pitch yaw # msg = mavlink1.MAVLink_rc_channels_override_message(master.target_system, master.target_component, 4000, 4000, 4000, 0, 0, 0, 0, 0) # msg.pack(mav) # buf = msg.get_msgbuf() # sleep(0.001) # master.write(buf) # master.write(buf) # master.write(buf) # master.write(buf) # master.write(buf) # Cria mensagens a partir do dialeto # m1 = mavlink1.MAVLink_param_set_message(master.target_system, master.target_component, "FLTMODE5", 7, mavlink1.MAV_PARAM_TYPE_INT32)
def print_packets(pcap): """Print out information about each packet in a pcap Args: pcap: dpkt pcap reader object (dpkt.pcap.Reader) """ total = [] # For each packet in the pcap process the contents for timestamp, buf in pcap: # Print out the timestamp in UTC #print('Timestamp: ', str(datetime.datetime.fromtimestamp(timestamp))) # Unpack the Ethernet frame (mac src/dst, ethertype) eth = dpkt.ethernet.Ethernet(buf) #print('Ethernet Frame: ', mac_addr(eth.src), mac_addr(eth.dst), eth.type) # Make sure the Ethernet data contains an IP packet if not isinstance(eth.data, dpkt.ip.IP): #print('Non IP Packet type not supported %s\n' % eth.data.__class__.__name__) continue # Now unpack the data within the Ethernet frame (the IP packet) # Pulling out src, dst, length, fragment info, TTL, and Protocol ip = eth.data # Pull out fragment information (flags and offset all packed into off field, so use bitmasks) do_not_fragment = bool(ip.off & dpkt.ip.IP_DF) more_fragments = bool(ip.off & dpkt.ip.IP_MF) fragment_offset = ip.off & dpkt.ip.IP_OFFMASK # Print out the IP info #print('IP: %s -> %s (len=%d ttl=%d DF=%d MF=%d offset=%d)' % \ # (inet_to_str(ip.src), inet_to_str(ip.dst), ip.len, ip.ttl, do_not_fragment, more_fragments, fragment_offset)) # Extrack UDP data from IP datagram udp = ip.data #print('UDP: %d -> %d (ulen=%d sum=%d)' % (udp.sport, udp.dport, udp.ulen, udp.sum) ) # Extrack MAVLINK data from UDP datagram mavlink_data = udp.data mavlink_object = apm.MAVLink(mavlink_data) # Wrong mavlink_msg = mavlink_object.parse_buffer(mavlink_data)[0] mavlink_msg_type = mavlink_msg.get_type() print(mavlink_msg_type) #for fieldname in mavlink_msg.get_fieldnames(): # print(fieldname) #for key in mavlink_msg: # print("%s: %s" %(key,mavlink_msg[key])) print("UDP: %i" % (len(udp.data))) test = mavlink_msg.get_msgbuf() print("MSG BUF: %i" % (len(test))) for item in test: print(str(item), end=" ") print("\n\n") #print("MAVLINK: %d bytes" %(len(mavlink_data))) #print("MSG: %s" %(mavlink_msg_type)) #print(mavlink_msg) #print() data = {} data["Time"] = timestamp data["IpSrc"] = inet_to_str(ip.src) data["IpDst"] = inet_to_str(ip.dst) data["UdpSrcP"] = int(udp.sport) data["UdpDstP"] = int(udp.dport) data["UdpUlen"] = int(udp.ulen) data["UdpSum"] = int(udp.sum) data["MAVL_type"] = mavlink_msg_type data["MAVL_len"] = len(mavlink_data) data["Raw"] = unicode(udp.data, errors='ignore') total.append(data) f = open("UDP_Report.txt", "w") f.write(json.dumps(total, indent=4)) f.close()
def my_main(): mav_master, mav_modes = init_mav() inject_mav = mavlink.MAVLink(nothing(), mav_master.target_system, 1) if len(sys.argv) < 2: print 'no relay server_ip:port, disable mavlink forward' mav_relay = empty_conn() else: mav_relay = mavutil.mavlink_connection( device="udpout:" + sys.argv[1], source_system=mav_master.target_system) try: #rtk_base = serial.Serial('/dev/ttyUSB0', 57600, timeout = 0) rtk_base = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) except serial.SerialException: rtk_base = None ts = time.time() qmi_ts = ts qmi_proc = None buf = "" count = 0 arm_count = 0 #buzzer = Buzzer() while True: cur_ts = time.time() msg = mav_master.recv_msg() if msg is not None and msg.get_type() != "BAD_DATA": data = msg.pack(inject_mav) buf = buf + data if len(buf) > 100: mav_relay.write(buf) buf = "" count = count + 1 if rtk_base is not None: rtcm_msg = rtk_base.recv_msg() if rtcm_msg is not None and rtcm_msg.get_type() == 'GPS_RTCM_DATA': print 'rtcm data', rtcm_msg.len mav_master.mav.send(rtcm_msg) msg = mav_relay.recv_msg() if msg is not None: msg_type = msg.get_type() if msg_type == "PING": print 'recv ping' out_msg = inject_mav.ping_encode(msg.time_usec, msg.seq, msg.get_srcSystem(), msg.get_srcComponent()) data = out_msg.pack(inject_mav) buf = buf + data elif msg_type == "COMMAND_LONG" and msg.command == 400 and msg.target_system == mav_master.target_system: #ARM_DSARM if msg.param1 == 0: print 'recv disarm' arm_count = 0 mav_master.mav.send(msg) else: print 'recv arm' if arm_count > 1: arm_count = 0 mav_master.mav.send(msg) else: #buzzer.play(5) arm_count = arm_count + 1 out_msg = inject_mav.command_ack_encode(400, 1) data = out_msg.pack(inject_mav) buf = buf + data out_msg = inject_mav.statustext_encode( 5, "Arm: try again") data = out_msg.pack(inject_mav) buf = buf + data elif msg_type != "BAD_DATA": mav_master.mav.send(msg) if cur_ts - ts > 0.02: ts = cur_ts if len(buf) > 0: mav_relay.write(buf) buf = "" count = count + 1 if qmi_proc is None: if cur_ts - qmi_ts > 5: try: qmi_proc = subprocess.Popen(['./cellular_info.sh'], stdout=subprocess.PIPE) except OSError as oops_err: print oops_err qmi_ts = cur_ts #try again later else: if qmi_proc.poll() is not None and qmi_proc.poll() == 0: cell_technology = None cell_id = None txt = qmi_proc.stdout.read() m = re.search( "Current:\n\tNetwork '([0-9a-zA-Z]+)': '-([0-9]+) dBm'", txt) if m is not None: cell_technology = m.group(1).lower() cell_rssi = int(m.group(2)) print 'network', cell_technology, cell_rssi m = re.search( "Cell ID: '([0-9]+)'\n\t\tMCC: '([0-9]+)'\n\t\tMNC: '([0-9]+)'\n\t\tTracking Area Code: '([0-9]+)'", txt) if m is not None: cell_id = int(m.group(1)) cell_mcc = int(m.group(2)) cell_mnc = int(m.group(3)) cell_tac = int(m.group(4)) print 'cell', cell_id, cell_mcc, cell_mnc, cell_tac if cell_technology is not None and cell_id is not None: msg = inject_mav.cellular_status_encode( cell_technology, cell_rssi, cell_mcc, cell_mnc, cell_tac, cell_id) data = msg.pack(inject_mav) buf = buf + data qmi_ts = time.time() qmi_proc = None
print( "Usage: pose_republisher.py [input_pose_topic] [output_pose_topic]" ) sys.exit(1) in_topic = sys.argv[1] out_topic = sys.argv[2] rospy.init_node("pose_republisher") mavlink_pub = rospy.Publisher("/mavlink/to", Mavlink, queue_size=20) pose_pub = rospy.Publisher(out_topic, PoseStamped, queue_size=10) time_sub = rospy.Subscriber("/mavlink/from", Mavlink, time_callback) # Setup mavlink instance f = fifo() mav = MAV_APM.MAVLink(f, srcSystem=1, srcComponent=1) while mavlink_pub.get_num_connections() <= 0: # wait for everything to initialize pass # Then start republishing from in_topic to out_topic pose_sub = rospy.Subscriber( in_topic, PoseStamped, pose_callback) # automatically republishes to out_topic rospy.spin() except rospy.ROSInterruptException: pass
def __init__(self): self.buf = [] def write(self, data): self.buf += data return len(data) def read(self): return self.buf.pop(0) # we will use a fifo as an encode/decode buffer f = fifo() # create a mavlink instance, which will do IO on file object 'f' mav = mavlink.MAVLink(f) # set the WP_RADIUS parameter on the MAV at the end of the link #mav.param_set_send(7, 1, "WP_RADIUS", 101, mavlink.MAV_PARAM_TYPE_REAL32) #mav.heartbeat_send(1, 2, 3, 4, 5, 6) # alternatively, produce a MAVLink_param_set object # this can be sent via your own transport if you like #m = mav.param_set_encode(7, 1, "WP_RADIUS", 101, mavlink.MAV_PARAM_TYPE_REAL32) m = mav.heartbeat_encode(1, 2, 3, 4, 5, 6) # get the encoded message as a buffer b = m.get_msgbuf() # decode an incoming message m2 = mav.decode(b)
def __init__(self): self.buf = [] def write(self, data): self.buf += data return len(data) def read(self): return self.buf.pop(0) # Create a buffer class f = fifo() # Create the dialect mav = mavlink1.MAVLink(f) # Variables used to save the received variables from the APM board it_number = 1000 Theta_mot_1 = [0] * it_number Theta_mot_2 = [0] * it_number Theta_mot_3 = [0] * it_number Theta_mot_4 = [0] * it_number PWM1 = [0] * it_number PWM3 = [0] * it_number PWM2 = [0] * it_number PWM4 = [0] * it_number FT = [0] * it_number