def main():
signal.signal(signal.SIGINT, signal_handler)
os.environ['MAVLINK20'] = '1'
mavutil.set_dialect('common')
args = parse_args()
if args['udp']:
mav = mavutil.mavlink_connection('udpout:' + args['udp'],
source_system=args['system'],
source_component=args['component'])
elif args['serial']:
mav = mavutil.mavlink_connection(args['serial'],
baud=57600,
source_system=args['system'],
source_component=args['component'])
else:
sys.exit(1)
if not args['noheartbeat']:
start_heartbeats(mav)
while True:
msg = mav.recv_match(blocking=True)
if (args['verbose']):
msg_string = "{:s} from {:d}.{:d}".format(msg.get_type(),
msg.get_srcSystem(),
msg.get_srcComponent())
try:
dest_string = " to {:d}.{:d}".format(msg.target_system,
msg.target_component)
msg_string += dest_string
except:
pass
print(msg_string)
else:
print(msg.get_type())
sys.stdout.flush()
def __init__(self, connection_string, cb=None):
mavutil.set_dialect("ardupilotmega")
self.m = mavutil.mavlink_connection(connection_string)
self.cb = cb
self.running = True
read_th = threading.Thread(target=self.read_thread)
read_th.start()
def start_connection(args):
if not args['mavlink1']:
os.environ['MAVLINK20'] = '1'
mavutil.set_dialect('common')
if args['udp']:
mav = mavutil.mavlink_connection('udpout:' + args['udp'],
source_system=args['system'], source_component=args['component'])
elif args['serial']:
mav = mavutil.mavlink_connection(args['serial'], baud=57600,
source_system=args['system'], source_component=args['component'])
else:
sys.exit()
return mav
def connect(self):
mavutil.set_dialect("ardupilotmega")
self.autopilot = mavutil.mavlink_connection('udpin:localhost:14550')
self.autopilot.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" %
(self.autopilot.target_system, self.autopilot.target_system))
msg = None
# wait for autopilot connection
while msg is None:
msg = self.autopilot.recv_msg()
print(msg)
def main(imagePath, imageName, debug, gridNum, masks):
print(imagePath)
global pixhawk
# Incremental for the image counter
count = 0
# Specifies the pixhawk dialect to the mavlink
mavutil.set_dialect("ardupilotmega")
# Establishes connection to pixhawk
try: #Port pixhawk connects to
pixhawk = mavutil.mavlink_connection("COM4", autoreconnect=True)
except:
print("Could not connect")
# These funcs will either init the avionics data or give them
# the default values if pixhawk is not connected
getPos()
getAttitude()
count = 0
if (not debug):
#Removes old preprocessed files
if (os.path.exists(imagePath + "Preprocessed/")):
shutil.rmtree(imagePath + "Preprocessed/")
#Makes a folder to put preprocessed files into
os.mkdir(imagePath + "Preprocessed/")
#Loops through all the images in the folder
print(imagePath)
for f in os.listdir(imagePath):
if f.lower().endswith(".jpg"):
#Save the previous count to calculate how many output files are written
tempCount = count
#Returns a new count and outputs json file with images of tracked images
start_time = time.time()
count = track(imagePath, f, debug, gridNum, masks, count)
elpased_time = time.time() - start_time
#Calculates output files written and prints it to screen
print("Added " + str(count - tempCount) + " files in " +
str(round(elpased_time, 2)) + "seconds")
else:
track(imagePath, imageName, debug, gridNum, masks, count)
def setup_gcs_link(self, gcs_port, gcs_baud=115200):
"""
This sets up the connection to the GCS Pixhawk.
Parameters
-----------
uav_port : str
Serial port where GCS Pixhawk is connected (via usb cable)
uav_baud: int, optional
The baud rate. Default is 115200
"""
mavutil.set_dialect(self.dialect)
self.gcs = mavutil.mavlink_connection(gcs_port, gcs_baud)
self.gcs_port = gcs_port
self.gcs_baud = gcs_baud
def setup_uav_link(self, uav_port, uav_baud=56700):
"""
This sets up the connection to the UAV.
Parameters
-----------
uav_port : str
Serial port where UAV is connected (via telemetry radio)
uav_baud: int, optional
The baud rate. Default is 56700
"""
mavutil.set_dialect(self.dialect)
self.uav = mavutil.mavlink_connection(uav_port, uav_baud)
self.uav_port = uav_port
self.uav_baud = uav_baud
def setup_gcs_link(self, gcs_port, gcs_baud=115200):
"""
This sets up the connection to the GCS Pixhawk.
Parameters
-----------
uav_port : str
Serial port where GCS Pixhawk is connected (via usb cable)
uav_baud: int, optional
The baud rate. Default is 115200
"""
mavutil.set_dialect(self.dialect)
self.gcs = mavutil.mavlink_connection(gcs_port, gcs_baud)
self.gcs_port = gcs_port
self.gcs_baud = gcs_baud
def setup_uav_link(self, uav_port, uav_baud=56700):
"""
This sets up the connection to the UAV.
Parameters
-----------
uav_port : str
Serial port where UAV is connected (via telemetry radio)
uav_baud: int, optional
The baud rate. Default is 56700
"""
mavutil.set_dialect(self.dialect)
self.uav = mavutil.mavlink_connection(uav_port, uav_baud)
self.uav_port = uav_port
self.uav_baud = uav_baud
def main(ip, loc, system, component):
#import pdb; pdb.set_trace()
args = Args(ip, loc, system, component, dialect)
test_dest = 40.003730, -105.258362, 1000
calculate_azimuth_elevation(args.loc, test_dest)
# setup mavlink mavfile (UDP)
# Give system, component, IP connecting, port connecting to
mav = mavutil.mavlink_connection('udpout:' + args.ip,
source_system=args.mavsystem,
source_component=args.mavcomponent)
# hand mavfile to mavconn constructor
mavutil.set_dialect(args.dialect)
mavconn = MAVLinkConnection(mav)
with mavconn as m:
mavconn.add_timer(1, heartbeat)
def Execute(self):
mavutil.set_dialect("ardupilotmega")
autopilot = mavutil.mavlink_connection('tcp:localhost:5762')
msg = None
# wait for autopilot connection
while msg is None:
msg = autopilot.recv_msg()
print msg
# The values of these heartbeat fields is not really important here
# I just used the same numbers that QGC uses
# It is standard practice for any system communicating via mavlink emit the HEARTBEAT message at 1Hz! Your autopilot may not behave the way you want otherwise!
autopilot.mav.heartbeat_send(
6, # type
8, # autopilot
192, # base_mode
0, # custom_mode
4, # system_status
3 # mavlink_version
)
autopilot.mav.command_long_send(
1, # autopilot system id
1, # autopilot component id
400, # command id, ARM/DISARM
0, # confirmation
1, # arm!
0,
0,
0,
0,
0,
0 # unused parameters for this command
)
time.sleep(2)
autopilot.set_mode_manual()
autopilot.mav.rc_channels_override_send(autopilot.target_system,
autopilot.target_component, 0,
2000, 2000, 0, 0, 0, 0, 0)
time.sleep(10)
global inputs
inputs = "1100"
if inputs == "1100":
self.FSM.ToTransition("toShoot")
self.FSM.Execute()
def __setMavlinkDialect(self, ap):
mavutil.mavlink = None # reset previous dialect
self.uas = UASInterfaceFactory.getUASInterface(ap)
self.uas.mavlinkMessageTxSignal.connect(self.sendMavlinkMessage)
if ap in MAVLINK_DIALECTS:
print('Set dialect to: {} ({})'.format(MAVLINK_DIALECTS[ap], ap))
mavutil.set_dialect(MAVLINK_DIALECTS[ap])
elif ap != mavlink.MAV_AUTOPILOT_INVALID:
# default to common
print('Set dialect to common for unknown AP type:', ap)
mavutil.set_dialect(
MAVLINK_DIALECTS[mavlink.MAV_AUTOPILOT_GENERIC])
# Hot patch after setting mavlink dialect on the fly
self.connection.mav = mavutil.mavlink.MAVLink(
self.connection,
srcSystem=self.connection.source_system,
srcComponent=self.connection.source_component)
self.connection.mav.robust_parsing = self.connection.robust_parsing
self.connection.WIRE_PROTOCOL_VERSION = mavutil.mavlink.WIRE_PROTOCOL_VERSION
def __init__(self, port=None, debug_log_to_console=False):
super(AMavlink, self).__init__()
self._initialize_logger(debug_log_to_console)
mavutil.set_dialect('ardupilotmega')
if port is None:
self._port = 14551
else:
self._port = int(port)
self._default_host = '127.0.0.1'
self._mavutil = None
self._connect()
self.command = AMavlinkCommand(self)
self.eeprom = AMavlinkEeprom(self)
self.flightmode = AMavlinkFlightmode(self)
self.heartbeat = AMavlinkHeartbeat(self)
self.location = AMavlinkLocation(self)
self.message = AMavlinkMessage(self)
self.param = AMavlinkParam(self)
self.rcinput = AMavlinkRCInput(self)
self.system = AMavlinkSystem(self)
self.tune = AMavlinkTune(self)
import math
import inspect
import threading
import os
import ctypes
SIMULATOR_UDP = 'udp:127.0.0.1:14550'
UDP_IP_LOCAL = "127.0.0.1"
UDP_PORT = 2992
UDP_IP_650 = 'tcp:192.168.0.210:20002'
sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP_LOCAL, UDP_PORT))
master = mavutil.mavlink_connection(UDP_IP_650, dialect = "ardupilotmega")
mavutil.set_dialect("ardupilotmega")
class simpleVectorVelocity:
vx = 0
vy = 0
vxN = 0
vyN = 0
def __init__(self, vx, vy):
self.vx = vx
self.vy = vy
def magnitudeIs(self):
return math.sqrt(math.pow(self.vx,2) + math.pow(self.vy,2))
def phaseToXIs(self):
return math.atan2(self.vy, self.vx) * 180 / math.pi
def normalize(self):
def data(q, m):
# Create a Trafic manager instance
TM = TrafficManager()
master = 'NONE'
logger = logging.getLogger()
mlog = None
while True:
# get messages from ac
if master != 'NONE' or master != 'END':
update_position(q, TM, master, mlog, forwarding)
# execute commands from gs
if len(m) > 0:
logger.info('IC {}: Message: {}'.format(ac, str(m[0])))
if m[0][1] != '-1':
master, mlog = completeCommands(q, m[0], TM, master, mlog)
if "NEW_AIRCRAFT" in m[0]:
global sim_type
sim_type = m[0][10]
logger.info('IC {}: Waiting for Icarous to load.'.format(ac))
# setup the .tlog
d = int(time.time())
d_formated = time.strftime("%Y-%m-%d_%H-%M-%S",
time.localtime(d))
log = 'LogFiles/flight_log_ac_{0}_{1}.tlog'.format(
ac, d_formated)
with open(log, 'w') as f:
f.write('')
# connect to the log
mlog = mavutil.mavlink_connection(log,
dialect='ardupilotmega',
baud=BAUD,
write=True,
append=True)
mavutil.set_dialect("ardupilotmega")
# give time for icarous to load
time.sleep(12)
logger.info('IC {}: Loading parameters.'.format(ac))
elif 'PLAYBACK' in m[0]:
if 'START' in m[0]:
master = 'FILE'
log = m[0][4]
# check file exists
try:
f = open('LogFiles/' + log, 'r')
except Exception as e:
q.put(
'{"name":"IC_PLAYBACK", "INFO": "Playback error: File Not Found.'
+ str(e) + '", "ecode":"1"}')
return
logger.info('IC Playback: Opening File: {}'.format(log))
# get file type, tlog or mlog
if 'tlog' in log[-4:]:
global logplayer
logplayer = P.LogPlayer(log, 'tlog')
logger.info(
'IC Playback: Created Log Player {}'.format(
logplayer))
logplayer.getMessages()
logger.info('IC Playback : {} {}'.format(
log, logplayer.total_messages))
elif 'mlog' in log[-4:]:
logplayer = P.LogPlayer(log, 'mlog')
logger.info(
'IC Playback : Created Log Player {}'.format(
logplayer))
logplayer.getMessages()
else:
logger.warning(
'IC Playback: Playback error: Unknown file type. {}'
.format(log[-4:]))
q.put(
'{"name":"IC_PLAYBACK", "INFO": "Playback error: Unknown file type.", "ecode":"2"}'
)
quit()
elif 'PLAY' in m[0]:
logplayer.Play()
elif 'SHUTDOWN' in m[0]:
q.put('{"AIRCRAFT":"PLAYBACK", "name":"SHUT_DOWN"}')
logger.info('IC Playback: Shutdown recieved')
logplayer = None
q.close()
master = 'END'
elif 'REW' in m[0]:
logplayer.Rew()
elif 'FF' in m[0]:
logplayer.FF()
elif 'SKIP' in m[0]:
logplayer.SkipForward()
# Remove the completed command from the list
m.pop(0)
logger.info('IC {0}: List {0}: {1}'.format(ac, str(m)))
# if shutdown recieved exit the process
if master == 'END':
logger.info('IC {}: calling quit'.format(ac))
quit()
def completeCommands(q, msg, TM, master, mlog):
global forwarding
logger = logging.getLogger()
if len(msg) > 0:
# parse the message
message = msg
consumer_message = message[2:]
if 'NEW_AIRCRAFT' in consumer_message:
global ac
ac = message[1]
master, pidI, pidA = CF.requestNewAircraft(consumer_message, 57600,
UDP_PORT_2, HOST)
logger.info('IC {}: master: {} pidI: {} pidA: {}'.format(
ac, master, pidI, pidA))
ac_pids.append(pidI)
ac_pids.append(pidA)
elif 'HITL' in consumer_message:
BAUD = consumer_message[3]
IP = consumer_message[5]
PORT = consumer_message[7]
COMP = consumer_message[9]
ac = consumer_message[1]
if COMP != 'Default':
MF.setTargetComponent(int(COMP))
print('ic processes', COMP)
master = CF.connectToHardwareIP(consumer_message, IP, PORT, BAUD)
d = int(time.time())
d_formated = time.strftime("%Y-%m-%d_%H-%M-%S", time.localtime(d))
log = 'LogFiles/flight_log_ac_{0}_{1}.tlog'.format(ac, d_formated)
with open(log, 'w') as f:
f.write('')
mlog = mavutil.mavlink_connection(log,
dialect='ardupilotmega',
baud=BAUD,
write=True,
append=True)
mavutil.set_dialect("ardupilotmega")
global hitl
hitl = True
if master != 'NONE':
logger.info('IC {}: master {}'.format(ac, master))
else:
logger.info('IC {0}: Connection Failed'.format(ac))
q.put('{"name":"HITL", "INFO":"CONNECTION_FAILED"}')
elif 'HITL_DISCONNECT' in consumer_message:
global has_heartbeat
has_heartbeat = True
master.close()
q.close()
q.cancel_join_thread()
master = 'END'
logger.info('IC {0}: Disconnected'.format(ac))
elif 'REQUEST_WAYPOINTS' in consumer_message:
logger.info('')
logger.info(
'**************************************************************'
)
msg_in = MF.requestCount(master, mlog, forwarding)
if str(msg_in) != 'NONE':
msg = CF.requestWaypoints(msg_in, consumer_message[1], master,
mlog, forwarding)
q.put(str(msg))
else:
q.put(
'{"name":"WAYPOINT_REQUEST", "INFO":"FAIL. Message_Timeout_Reached"}'
)
logger.info(
'WAYPOINT_REQUEST : FAIL : Message_Timeout_Reached')
logger.info(
'**************************************************************'
)
logger.info('')
elif 'REQUEST_FENCE' in consumer_message:
logger.info('')
logger.info(
'**************************************************************'
)
msg_in = MF.requestCountF(master, mlog, forwarding)
if str(msg_in) != 'NONE':
msg = CF.requestVert(msg_in, consumer_message[1], master, mlog,
forwarding)
q.put(str(msg))
else:
q.put(
'{"name":"VERT_REQUEST", "INFO":"FAIL. Message_Timeout_Reached"}'
)
logger.info('VERT_REQUEST : FAIL : Message_Timeout_Reached')
logger.info(
'**************************************************************'
)
logger.info('')
elif 'REQUEST_REPLAN' in consumer_message:
logger.info('')
logger.info(
'**************************************************************'
)
msg_in = MF.requestCountR(master, mlog, forwarding)
if str(msg_in) != 'NONE':
msg = CF.requestReplan(msg_in, consumer_message[1], master,
mlog, forwarding)
q.put(str(msg))
else:
q.put(
'{"name":"WAYPOINT_REQUEST", "INFO":"FAIL. Message_Timeout_Reached"}'
)
logger.info(
'WAYPOINT_REQUEST : FAIL : Message_Timeout_Reached')
logger.info(
'**************************************************************'
)
logger.info('')
elif 'ADSB_VEHICLE' in consumer_message:
MF.sendTraffic(consumer_message, master)
elif 'LOAD_FLIGHT_PLAN' in consumer_message:
CF.loadFlightPlan(ac, consumer_message, q, master, mlog,
forwarding)
elif 'FLIGHT_STARTED' in consumer_message:
icarous_flag = consumer_message[-1]
launch = consumer_message[-2]
logger.info('IC {}: Icarous Flag: {}'.format(ac, icarous_flag))
MF.startFlight(icarous_flag, launch, master, mlog, forwarding)
logger.info('IC {}: Flight Started'.format(ac))
q.put('{"AIRCRAFT" : ' + consumer_message[-3] +
', "TYPE":"STARTFLIGHT", "INFO": "SUCCESS"}')
elif 'ADD_TRAFFIC' in consumer_message:
# print(consumer_message)
message.append(master)
TM.add_traffic(message)
elif 'FILE_TRAFFIC' in consumer_message:
t_id = consumer_message[1]
# open the file
dir = os.path.dirname(os.path.realpath(__file__))
filename = consumer_message[2]
path = dir + '/../..' + filename
# get the data
with open(path) as f:
line = f.readline()
logger.info('IC {}: File Add Traffic {}'.format(ac, line))
x = line.replace('\n', '').split(' ')
message = [
'AIRCRAFT', ac, 'ADD_TRAFFIC', t_id, x[0], x[1], 0, 0,
x[2], x[3], x[4], 0, x[5], t_id, master
]
TM.add_traffic(message)
f.close()
elif 'REMOVE_TRAFFIC' in consumer_message:
TM.remove_traffic(consumer_message)
elif 'LOAD_GEOFENCE' in consumer_message:
CF.loadGeoFence(ac, consumer_message, q, master, mlog, forwarding)
elif 'REMOVE_GEOFENCE' in consumer_message:
# Not really working, just replacing the fence with another fence with no points
index = consumer_message[6]
floor = 2
roof = 100
MF.removeGF(index, floor, roof, master)
logger.info('IC {}: Fence Removed'.format(ac))
elif 'SHUTDOWN' in consumer_message:
master.close()
q.close()
CF.shutdownProcesses(ac_pids)
master = 'END'
elif 'RESET_ICAROUS' in consumer_message:
MF.resetIcarous(master)
elif 'UPDATE_PARAM_LIST' in consumer_message:
print('Requesting Params')
MF.fetchParams(master)
logger.info('IC {}: trying to fetch params'.format(ac))
elif 'CHANGE_PARAM' in consumer_message:
logger.info('IC {}: trying to change params'.format(ac))
MF.changeParamFloat(consumer_message, master, mlog, forwarding)
q.put('{"name":"CHANGE_PARAM", "INFO": "SUCCESS. ' +
str(consumer_message) + '"}')
logger.info('IC {}: Change Params Success.'.format(ac))
elif 'LOAD_WP_FILE' in consumer_message:
CF.loadFlightPlanFile(ac, consumer_message, q, master, mlog)
elif 'LOAD_GF_FILE' in consumer_message:
CF.loadGeoFenceFile(ac, consumer_message, q, master, mlog)
elif 'LOAD_PARAM_FILE' in consumer_message:
CF.loadParamFile(ac, consumer_message, q, master, mlog)
elif 'SAVE' in consumer_message:
CF.saveFile(ac, consumer_message, q, master, mlog)
elif 'RADAR' in consumer_message:
# ['RADAR', '1']
logger.info('IC {}: Sending Radar Command:'.format(ac),
consumer_message)
MF.sendUser3(consumer_message[1], master)
elif 'FORWARD' in consumer_message:
if consumer_message[1] == 'STOP':
forwarding.close()
forwarding = None
else:
device = 'udp:' + consumer_message[1] + ':' + consumer_message[
2]
baud = consumer_message[3]
print(device, baud)
forwarding = mavutil.mavlink_connection(
device, baud=baud, input=False, dialect='ardupilotmega')
else:
print('UNKNOWN MESSAGE: ', consumer_message)
return master, mlog
def fileMerge(filenames, newFileName, inpath, outpath):
all_unsorted = []
for i in range(len(filenames)):
x = bytearray(struct.pack('>Q', i+1))
# connect to the file
print('In:', inpath + filenames[i])
try:
tlog = mavutil.mavlink_connection(
inpath + filenames[i], dialect='ardupilotmega', baud=57600, write=False, planner_format=False, notimestamps=True)
mavutil.set_dialect("ardupilotmega")
except AttributeError:
print(
'File not found. Please check the filename and directory.', inpath+filenames[i])
return
# loop over all of the messages
unsorted = []
msg = ' '
while msg:
# parse the timestamp for sorting
t = tlog.f.read(8)
if len(t) != 8:
break
(timestamp,) = struct.unpack('>Q', t)
timestamp = timestamp * 1.0e-6
# get the message and convert it back to bytearray
msg = tlog.recv_msg()
if len(str(msg)) > 0:
msg = msg.get_msgbuf()
# create a list containing [id, timestamp, message]
group = [x, timestamp, msg]
# add it to unsorted
unsorted.append(group)
else:
break
# add unsorted to all unsorted
all_unsorted.append(unsorted)
tlog.close()
# merge and sort based on timestamp
m_top = [[all_unsorted.index(x)]+x.pop(0) for x in all_unsorted]
m_sorted = []
while True:
# compare them
m_top.sort(key=itemgetter(2))
# put the smallest one into m_sorted
small = m_top.pop(0)
m_sorted.append(small[1:])
# get the next value from the same array that was smallest
if len(all_unsorted[small[0]]) > 0:
m_next = small[0]
m_top.append([m_next]+all_unsorted[m_next].pop(0))
# check if all of the sub lists are empty
z = [len(x) for x in all_unsorted if len(x) != 0]
if len(z) == 0:
break
print('Out: ' + outpath + newFileName)
open(outpath + newFileName, 'a').close()
mlog = mavutil.mavlink_connection(
outpath + newFileName, dialect='ardupilotmega', baud=57600, write=True, planner_format=False,)
for x in m_sorted:
# convert timestamp back to bit array
timestamp = bytearray(struct.pack('>Q', int(x[1]*1.0e6)))
# merge the bitarrays back together
line = x[0]+timestamp+x[2]
mlog.write(line)
mlog.close()
print('Done. File\'s merged into:', newFileName)
action='append',
default=[],
help='Load the specified module. Can be used multiple times, or with a comma separated list')
parser.add_option("--mav09", action='store_true', default=False, help="Use MAVLink protocol 0.9")
parser.add_option("--auto-protocol", action='store_true', default=False, help="Auto detect MAVLink protocol version")
parser.add_option("--nowait", action='store_true', default=False, help="don't wait for HEARTBEAT on startup")
parser.add_option("--continue", dest='continue_mode', action='store_true', default=False, help="continue logs")
parser.add_option("--dialect", default="ardupilotmega", help="MAVLink dialect")
parser.add_option("--rtscts", action='store_true', help="enable hardware RTS/CTS flow control")
(opts, args) = parser.parse_args()
if opts.mav09:
os.environ['MAVLINK09'] = '1'
from pymavlink import mavutil, mavparm
mavutil.set_dialect(opts.dialect)
# global mavproxy state
mpstate = MPState()
mpstate.status.exit = False
mpstate.command_map = command_map
mpstate.continue_mode = opts.continue_mode
if opts.speech:
# start the speech-dispatcher early, so it doesn't inherit any ports from
# modules/mavutil
load_module('speech')
if not opts.master:
serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*',"*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*"])
if len(serial_list) == 1:
from pymavlink import mavutil
import time
mavutil.set_dialect("video_monitor")
# create a connection to FMU
hoverGames = mavutil.mavlink_connection("/dev/ttyUSB0", baud=921600)
# wait for the heartbeat message to find the system id
hoverGames.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" %
(hoverGames.target_system, hoverGames.target_component))
counter = 0
#send custom mavlink message
while (True):
hoverGames.mav.video_monitor_send(
timestamp=int(time.time() * 1e6), # time in microseconds
info=b'Salut!',
lat=counter,
lon=231234567,
no_people=counter,
confidence=0.357)
counter += 1
print("The custom mesage with the number %u was sent it!!!!" % (counter))
time.sleep(1.0)
#!/usr/bin/python
# -*- coding: utf-8 -*-
import struct
import binascii
import pymavlink.dialects.v10.lapwing as mavlink
import pymavlink.mavutil as mavutil
mavutil.set_dialect("lapwing")
# from tkinter import ttk
# from tkinter import *
# from tkinter.ttk import *
from tkinter import *
from tkinter import messagebox
# create in and out communication channels
mavin = mavutil.mavlink_connection("udpin:localhost:14551")
mavout = mavutil.mavlink_connection("udpout:localhost:14556")
RECV_TIMEOUT = 2.0
RECV_TRIES = 4
CHANNEL_NUMBER = 4
ENTRY_WIDTH = 12
LABEL_WIDTH = 3
def mavlink_wait_param_timeout(param_name): #{{{
ret = None
print("Trying to get:", param_name)
def initMAVProxy():
from optparse import OptionParser
parser = OptionParser("mavproxy.py [options]")
parser.add_option("--master", dest="master", action='append',
metavar="DEVICE[,BAUD]", help="MAVLink master port and optional baud rate",
default=[])
parser.add_option("--out", dest="output", action='append',
metavar="DEVICE[,BAUD]", help="MAVLink output port and optional baud rate",
default=[])
parser.add_option("--baudrate", dest="baudrate", type='int',
help="default serial baud rate", default=115200)
parser.add_option("--sitl", dest="sitl", default=None, help="SITL output port")
parser.add_option("--streamrate",dest="streamrate", default=4, type='int',
help="MAVLink stream rate")
parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
default=255, help='MAVLink source system for this GCS')
parser.add_option("--target-system", dest='TARGET_SYSTEM', type='int',
default=1, help='MAVLink target master system')
parser.add_option("--target-component", dest='TARGET_COMPONENT', type='int',
default=1, help='MAVLink target master component')
parser.add_option("--logfile", dest="logfile", help="MAVLink master logfile",
default='mav.tlog')
parser.add_option("-a", "--append-log", dest="append_log", help="Append to log files",
action='store_true', default=False)
parser.add_option("--quadcopter", dest="quadcopter", help="use quadcopter controls",
action='store_true', default=False)
parser.add_option("--setup", dest="setup", help="start in setup mode",
action='store_true', default=False)
parser.add_option("--nodtr", dest="nodtr", help="disable DTR drop on close",
action='store_true', default=False)
parser.add_option("--show-errors", dest="show_errors", help="show MAVLink error packets",
action='store_true', default=False)
parser.add_option("--speech", dest="speech", help="use text to speach",
action='store_true', default=False)
parser.add_option("--num-cells", dest="num_cells", help="number of LiPo battery cells",
type='int', default=0)
parser.add_option("--aircraft", dest="aircraft", help="aircraft name", default=None)
parser.add_option("--cmd", dest="cmd", help="initial commands", default=None)
parser.add_option("--console", action='store_true', help="use GUI console")
parser.add_option("--map", action='store_true', help="load map module")
parser.add_option(
'--load-module',
action='append',
default=[],
help='Load the specified module. Can be used multiple times, or with a comma separated list')
parser.add_option("--mav09", action='store_true', default=False, help="Use MAVLink protocol 0.9")
parser.add_option("--auto-protocol", action='store_true', default=False, help="Auto detect MAVLink protocol version")
parser.add_option("--nowait", action='store_true', default=False, help="don't wait for HEARTBEAT on startup")
parser.add_option("--continue", dest='continue_mode', action='store_true', default=False, help="continue logs")
parser.add_option("--dialect", default="ardupilotmega", help="MAVLink dialect")
parser.add_option("--rtscts", action='store_true', help="enable hardware RTS/CTS flow control")
(opts, args) = parser.parse_args()
mavproxy.opts=opts
if opts.mav09:
os.environ['MAVLINK09'] = '1'
from pymavlink import mavutil, mavparm
mavutil.set_dialect(opts.dialect)
# global mavproxy state
mpstate = MPState()
mavproxy.mpstate=mpstate
mpstate.status.exit = False
mpstate.command_map = command_map
mpstate.continue_mode = opts.continue_mode
if opts.speech:
# start the speech-dispatcher early, so it doesn't inherit any ports from
# modules/mavutil
load_module('speech')
if not opts.master:
serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*',"*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*"])
if len(serial_list) == 1:
opts.master = [serial_list[0].device]
else:
print('''
Please choose a MAVLink master with --master
For example:
--master=com14
--master=/dev/ttyUSB0
--master=127.0.0.1:14550
Auto-detected serial ports are:
''')
for port in serial_list:
print("%s" % port)
sys.exit(1)
# container for status information
mpstate.status.target_system = opts.TARGET_SYSTEM
mpstate.status.target_component = opts.TARGET_COMPONENT
mpstate.mav_master = []
# open master link
for mdev in opts.master:
if ',' in mdev and not os.path.exists(mdev):
port, baud = mdev.split(',')
else:
port, baud = mdev, opts.baudrate
m = mavutil.mavlink_connection(port, autoreconnect=True, baud=int(baud))
m.mav.set_callback(master_callback, m)
if hasattr(m.mav, 'set_send_callback'):
m.mav.set_send_callback(master_send_callback, m)
if opts.rtscts:
m.set_rtscts(True)
m.linknum = len(mpstate.mav_master)
m.linkerror = False
m.link_delayed = False
m.last_heartbeat = 0
m.last_message = 0
m.highest_msec = 0
mpstate.mav_master.append(m)
mpstate.status.counters['MasterIn'].append(0)
# log all packets from the master, for later replay
open_logs()
# open any mavlink UDP ports
for p in opts.output:
if ',' in p and not os.path.exists(p):
port, baud = p.split(',')
else:
port, baud = p, opts.baudrate
mpstate.mav_outputs.append(mavutil.mavlink_connection(port, baud=int(baud), input=False))
if opts.sitl:
mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False)
mpstate.settings.numcells = opts.num_cells
mpstate.settings.streamrate = opts.streamrate
mpstate.settings.streamrate2 = opts.streamrate
mavproxy.msg_period = mavutil.periodic_event(1.0/15)
mavproxy.heartbeat_period = mavutil.periodic_event(1)
mavproxy.battery_period = mavutil.periodic_event(0.1)
mavproxy.heartbeat_check_period = mavutil.periodic_event(0.33)
mpstate.rl = rline.rline("MAV> ", mpstate)
if opts.setup:
mpstate.rl.set_prompt("")
if 'HOME' in os.environ and not opts.setup:
start_script = os.path.join(os.environ['HOME'], ".mavinit.scr")
if os.path.exists(start_script):
run_script(start_script)
if opts.aircraft is not None:
start_script = os.path.join(opts.aircraft, "mavinit.scr")
if os.path.exists(start_script):
run_script(start_script)
else:
print("no script %s" % start_script)
if not opts.setup:
# some core functionality is in modules
standard_modules = ['log','rally','fence','param','relay',
'tuneopt','arm','mode','calibration','rc','wp','auxopt','quadcontrols','test']
for m in standard_modules:
load_module(m, quiet=True)
if opts.console:
process_stdin('module load console')
if opts.map:
process_stdin('module load map')
for module in opts.load_module:
modlist = module.split(',')
for mod in modlist:
process_stdin('module load %s' % mod)
if opts.cmd is not None:
cmds = opts.cmd.split(';')
for c in cmds:
process_stdin(c)
# run main loop as a thread
mpstate.status.thread = threading.Thread(target=main_loop)
mpstate.status.thread.daemon = True
mpstate.status.thread.start()
#!/usr/bin/python
# -*- coding: utf-8 -*-
import struct
import binascii
import pymavlink.dialects.v10.lapwing as mavlink
import pymavlink.mavutil as mavutil
mavutil.set_dialect("lapwing")
# from tkinter import ttk
# from tkinter import *
# from tkinter.ttk import *
from tkinter import *
from tkinter import messagebox
# create in and out communication channels
mavin = mavutil.mavlink_connection("udpin:localhost:14551")
mavout = mavutil.mavlink_connection("udpout:localhost:14556")
RECV_TIMEOUT = 2.0
RECV_TRIES = 4
CHANNEL_NUMBER = 4
ENTRY_WIDTH = 12
LABEL_WIDTH = 3
def mavlink_wait_param_timeout(param_name): #{{{
ret = None
print("Trying to get:", param_name)
m = mavin.recv_match(type="PARAM_VALUE", blocking=True, timeout=RECV_TIMEOUT)
def __init__(self, in_queue, out_queue):
super(MavlinkModule, self).__init__(in_queue, out_queue, "mavlink")
mavutil.set_dialect(self.config['dialect'])
self.connection_str = self.config['connection']
self.connection = None
gtri_xml_path="/home/michael/uav-arobs/gtri-uav/uav_resources/uav-messages/mavlink_message_definitions/v1.0/gtri_uav.xml"
UAV_SUBNET="192.168.90.255"
DEFAULT_UAV_PORT=14550
#TODO: need targets based on UAVID
uav_ids = [2,4]
ids_to_remove = []
uav_params = {}
target_component = 1
param_provider = "A/P" #ATMY or A/P
done_checking = False
previous_rcv_time = time.time()
mavutil.set_dialect("gtri_uav")
master = mavutil.mavlink_connection(
"udpin:" + UAV_SUBNET + ":" + str(DEFAULT_UAV_PORT), dialect="gtri_uav")
#"udpbcast:" + UAV_SUBNET + ":" + str(DEFAULT_UAV_PORT), dialect="gtri_uav")
out = mavutil.mavlink_connection(
"udpbcast:" + UAV_SUBNET + ":" + str(DEFAULT_UAV_PORT), dialect="gtri_uav")
def uint_to_float(val_int):
s = struct.pack(">I", val_int)
return float(struct.unpack(">f", s)[0])
def uint_to_int(val_int):
s = struct.pack(">I", val_int)
return struct.unpack(">i", s)[0]
def __init__(self, in_queue, out_queue):
super(MavlinkModule, self).__init__(in_queue, out_queue, "mavlink")
mavutil.set_dialect("ardupilotmega")
self.connection_str = 'tcp:127.0.0.1:5763'
self.connection = None
if args.debug:
print('')
env_cleanup = False # Environment cleanup flag
if MAVLINK20_ENV not in os.environ: # Check if MAVLINK20 is set globally
# Force pymavlink to use MAVLink v2.0 using environment variable
os.environ[MAVLINK20_ENV] = ''
if args.debug:
print('{0} environment variable is set'.format(MAVLINK20_ENV))
# It's a good idea to clean this flag for other programs
env_cleanup = True
# Forcing pymavlink to use common MAVLink dialect
# HINT: The result depends on the MAVLINK20 environment variable
mavutil.set_dialect('common')
if env_cleanup: # Environment cleanup needed
del os.environ[
MAVLINK20_ENV] # No need in this environmental variable further
if args.debug:
print('{0} environment variable is removed'.format(MAVLINK20_ENV))
# Importing mavlink module from pymavlink
from pymavlink.mavutil import mavlink
if args.debug:
print('MAVLink module loaded: {0}'.format(mavlink))
mavlink_con = mavutil.mavlink_connection(
args.url,
from pymavlink import mavutil
import time
mavutil.set_dialect("mav_gps")
# create a connection to FMU
hoverGames = mavutil.mavlink_connection("/dev/ttyUSB0", baud=921600)
# wait for the heartbeat message to find the system id
hoverGames.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" %(hoverGames.target_system, hoverGames.target_component))
counter = 0
#send custom mavlink message
while(True) :
timestamp = int(time.time() * 1e6)
time_utc_usec = timestamp + 1613692609599954
lat = 296603018
lon = -823160500
alt = 30100
alt_ellipsoid = 30100
s_variance_m_s = 0.3740
c_variance_rad = 0.6737
eph = 2.1060
epv = 3.8470
hdop = 0.8800
vdop = 1.3300
noise_per_ms = 101
jamming_indicator = 35
def __init__(self):
mavutil.set_dialect("pixhawk")
self._mavlink = mavutil.mavlink.MAVLink(None, )
print(("ERROR: mavproxy takes no position arguments; got (%s)" %
str(args)))
sys.exit(1)
# warn people about ModemManager which interferes badly with APM and Pixhawk
if os.path.exists("/usr/sbin/ModemManager"):
print(
"WARNING: You should uninstall ModemManager as it conflicts with APM and Pixhawk"
)
#set the Mavlink version, if required
set_mav_version(opts.mav10, opts.mav20, opts.auto_protocol,
opts.mavversion)
from pymavlink import mavutil, mavparm
mavutil.set_dialect(opts.dialect)
#version information
if opts.version:
import pkg_resources
version = pkg_resources.require("mavproxy")[0].version
print(
"MAVProxy is a modular ground station using the mavlink protocol")
print("MAVProxy Version: " + version)
sys.exit(1)
# global mavproxy state
mpstate = MPState()
mpstate.status.exit = False
mpstate.command_map = command_map
mpstate.continue_mode = opts.continue_mode
def main():
global exit
global mavl
global motorpwm
exit = False
(opts, args) = parse_args()
if len(args) != 0:
print("ERROR: %s takes no position arguments; got (%s)" %
(sys.argv[0], str(args)))
sys.exit(1)
set_mav_version(opts.mav10, opts.mav20)
mavutil.set_dialect(opts.dialect)
mavl = None
motorpwm = None
#version information
if opts.version:
#pkg_resources doesn't work in the windows exe build, so read the version file
try:
import pkg_resources
version = pkg_resources.require("motorvibe")[0].version
except:
start_script = os.path.join(os.environ['LOCALAPPDATA'],
"motorvibe", "version.txt")
f = open(start_script, 'r')
version = f.readline()
print("Motorvibe Version: " + version)
sys.exit(1)
def quit_handler(signum=None, frame=None):
global exit
print('Signal handler called with signal', signum)
if exit:
print('Clean shutdown impossible, forcing an exit')
sys.exit(0)
else:
if mavl:
del (mavl)
if motorpwm:
del (motorpwm)
exit = True
# Listen for kill signals to cleanly shutdown modules
fatalsignals = [signal.SIGTERM, signal.SIGINT]
try:
fatalsignals.append(signal.SIGHUP)
fatalsignals.append(signal.SIGQUIT)
except Exception:
pass
for sig in fatalsignals:
signal.signal(sig, quit_handler)
if opts.download_logs:
print(
"WARNING: Only download px4 logs for each cycle if using a high-speed datalink and not running long cycles."
)
mavl = MotorMavLink(opts.mav10, opts.mav20, opts.SOURCE_SYSTEM,
opts.SOURCE_COMPONENT, opts.TARGET_SYSTEM,
opts.TARGET_COMPONENT, opts.rtscts, opts.baudrate,
opts.master, opts.log)
motorpwm = MotorPWMDriver(opts.pwmchip, opts.pwmchan, opts.pwmperiod)
mavl.mav_drop_throttle()
sleep(5.0)
mavl.mav_drop_throttle()
sleep(5.0)
mavl.mav_drop_throttle()
sleep(5.0)
motorpwm.set_pwm(900000)
input_fd = open(opts.input, 'r')
#Main Loop
for pwm in input_fd.readlines():
print("Looping")
if not os.path.exists(opts.motor):
os.mkdir(opts.motor)
pwm = int(pwm.rstrip('\n'))
pwm_path = "{}/pwm_{}".format(opts.motor, pwm)
if not os.path.exists(pwm_path):
os.mkdir(pwm_path)
motorpwm.set_pwm(pwm)
mavl.mav_arm(
) #Unfortunately, we need to arm before we spinup, otherwise preflight checks will fail.
sleep(5.0) #Give it some time to ramp up and stabilize
start_time = time()
mavl.start_vibration_log("{}/{}".format(pwm_path, "vibe.mavlink.csv"))
while ((time() - start_time)) < opts.runtime:
sleep(0.5)
mavl.stop_vibration_log()
mavl.mav_disarm()
motorpwm.set_pwm(900000)
sleep(5.0)
#Store latest log number to a file for later extraction of sd card (since download of mavlink can take a LOOOONNNNGGG time.
mavl.px4_log_reset()
mavl.mav_download_px4_log_list()
while mavl.download_last_log_num == -1:
sleep(2)
lognum_fd = open("{}/lognum.txt".format(pwm_path), 'w')
lognum_fd.write(str(mavl.download_last_log_num))
lognum_fd.close()
#Download corresponding px4 log
if opts.download_logs:
mavl.mav_download_px4_log_latest("{}/{}".format(
pwm_path, "log.ulg"))
while mavl.download_filename is not None:
sleep(
5
) #Logs can take a long time to download: Give a good sleep to let the mavl runtime thread do it's business
input_fd.close()
#Cleanup
exit = True
del (mavl)
del (motorpwm)
sys.exit(0)
