116 lines
5.7 KiB
Python
116 lines
5.7 KiB
Python
#!/usr/bin/env python
|
|
from __future__ import print_function
|
|
|
|
import fnmatch
|
|
import os
|
|
import time
|
|
import paho.mqtt.client as mqtt
|
|
from pymavlink.mavextra import *
|
|
from argparse import ArgumentParser
|
|
import uuid
|
|
parser = ArgumentParser(description=__doc__)
|
|
|
|
parser.add_argument("--no-timestamps", dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
|
|
parser.add_argument("--planner", action='store_true', help="use planner file format")
|
|
parser.add_argument("--robust", action='store_true', help="Enable robust parsing (skip over bad data)")
|
|
parser.add_argument("-f", "--follow", action='store_true', help="keep waiting for more data at end of file")
|
|
parser.add_argument("--condition", default=None, help="select packets by condition")
|
|
parser.add_argument("-q", "--quiet", action='store_true', help="don't display packets")
|
|
parser.add_argument("-o", "--output", default=None, help="output matching packets to give file")
|
|
parser.add_argument("-p", "--parms", action='store_true', help="preserve parameters in output with -o")
|
|
parser.add_argument("--format", default=None, help="Change the output format between 'standard', 'json', 'csv' and 'mat'. For the CSV output, you must supply types that you want. For MAT output, specify output file with --mat_file")
|
|
parser.add_argument("--csv_sep", dest="csv_sep", default=",", help="Select the delimiter between columns for the output CSV file. Use 'tab' to specify tabs. Only applies when --format=csv")
|
|
parser.add_argument("--types", default=None, help="types of messages (comma separated with wildcard)")
|
|
parser.add_argument("--nottypes", default=None, help="types of messages not to include (comma separated with wildcard)")
|
|
parser.add_argument("--mat_file", dest="mat_file", help="Output file path for MATLAB file output. Only applies when --format=mat")
|
|
parser.add_argument("-c", "--compress", action='store_true', help="Compress .mat file data")
|
|
parser.add_argument("--dialect", default="ardupilotmega", help="MAVLink dialect")
|
|
parser.add_argument("--zero-time-base", action='store_true', help="use Z time base for DF logs")
|
|
parser.add_argument("--no-bad-data", action='store_true', help="Don't output corrupted messages")
|
|
parser.add_argument("--show-source", action='store_true', help="Show source system ID and component ID")
|
|
parser.add_argument("--show-seq", action='store_true', help="Show sequence numbers")
|
|
parser.add_argument("--show-types", action='store_true', help="Shows all message types available on opened log")
|
|
parser.add_argument("--source-system", type=int, default=None, help="filter by source system ID")
|
|
parser.add_argument("--source-component", type=int, default=None, help="filter by source component ID")
|
|
parser.add_argument("--link", type=int, default=None, help="filter by comms link ID")
|
|
parser.add_argument("--verbose", action='store_true', help="Dump messages in a much more verbose (but non-parseable) format")
|
|
parser.add_argument("--mav10", action='store_true', help="parse as MAVLink1")
|
|
parser.add_argument("--reduce", type=int, default=0, help="reduce streaming messages")
|
|
parser.add_argument("log", metavar="LOG")
|
|
parser.add_argument("--profile", action='store_true', help="run the Yappi python profiler")
|
|
parser.add_argument("--meta", action='store_true', help="output meta-data msgs even if not matching condition")
|
|
|
|
args = parser.parse_args()
|
|
|
|
if not args.mav10:
|
|
os.environ['MAVLINK20'] = '1'
|
|
|
|
from pymavlink import mavutil
|
|
|
|
mqttBroker ="localhost"
|
|
|
|
client = mqtt.Client("telemetrics")
|
|
client.connect(mqttBroker)
|
|
|
|
mlog = mavutil.mavlink_connection("udpin:localhost:14540", #config
|
|
planner_format=args.planner,
|
|
notimestamps=args.notimestamps,
|
|
robust_parsing=args.robust,
|
|
dialect=args.dialect,
|
|
zero_time_base=args.zero_time_base)
|
|
|
|
types = args.types
|
|
if types is not None:
|
|
types = types.split(',')
|
|
|
|
nottypes = args.nottypes
|
|
if nottypes is not None:
|
|
nottypes = nottypes.split(',')
|
|
|
|
def match_type(mtype, patterns):
|
|
'''return True if mtype matches pattern'''
|
|
for p in patterns:
|
|
if fnmatch.fnmatch(mtype, p):
|
|
return True
|
|
return False
|
|
|
|
# for DF logs pre-calculate types list
|
|
match_types=None
|
|
if types is not None and hasattr(mlog, 'name_to_id'):
|
|
for k in mlog.name_to_id.keys():
|
|
if match_type(k, types):
|
|
if nottypes is not None and match_type(k, nottypes):
|
|
continue
|
|
if match_types is None:
|
|
match_types = []
|
|
match_types.append(k)
|
|
|
|
while True:
|
|
m = mlog.recv_match(blocking=args.follow, type=match_types)
|
|
|
|
if not mavutil.evaluate_condition(args.condition, mlog.messages) and (
|
|
not (m.get_type() in ['FMT', 'FMTU', 'MULT','PARM','MODE'] and args.meta)):
|
|
continue
|
|
if args.source_system is not None and args.source_system != m.get_srcSystem():
|
|
continue
|
|
if args.source_component is not None and args.source_component != m.get_srcComponent():
|
|
continue
|
|
if args.link is not None and args.link != m._link:
|
|
continue
|
|
|
|
if types is not None and m.get_type() != 'BAD_DATA' and not match_type(m.get_type(), types):
|
|
continue
|
|
|
|
# Ignore BAD_DATA messages is the user requested or if they're because of a bad prefix. The
|
|
# latter case is normally because of a mismatched MAVLink version.
|
|
if m.get_type() == 'BAD_DATA' and (args.no_bad_data is True or m.reason == "Bad prefix"):
|
|
continue
|
|
|
|
timestamp = getattr(m, '_timestamp', 0.0)
|
|
#s = "%s.%02u: %s" % (time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(timestamp)), int(timestamp*100.0)%100, m)
|
|
m = str(m).split(" ", 1)
|
|
s = "%s: %s" % (m[0], m[1])
|
|
client.publish("a1/%s" % (m[0]), m[1])
|
|
|
|
##print(s)
|