udrone/server/bridge.py
2022-08-18 22:20:07 +03:00

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)