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
