86 lines
3.0 KiB
Python
86 lines
3.0 KiB
Python
|
#!/usr/bin/python
|
||
|
|
"""
|
||
|
|
Allows connection of the uBlox u-Center software to
|
||
|
|
a uBlox GPS device connected to a PX4 or Pixhawk device,
|
||
|
|
using Mavlink's SERIAL_CONTROL support to route serial
|
||
|
|
traffic to/from the GPS, and exposing the data to u-Center
|
||
|
|
via a local TCP connection.
|
||
|
|
|
||
|
|
@author: Matthew Lloyd (github@matthewlloyd.net)
|
||
|
|
"""
|
||
|
|
from __future__ import print_function
|
||
|
|
|
||
|
|
from pymavlink import mavutil
|
||
|
|
from argparse import ArgumentParser
|
||
|
|
import socket
|
||
|
|
|
||
|
|
|
||
|
|
def main():
|
||
|
|
parser = ArgumentParser(description=__doc__)
|
||
|
|
parser.add_argument("--mavport", required=True,
|
||
|
|
help="Mavlink port name")
|
||
|
|
parser.add_argument("--mavbaud", type=int,
|
||
|
|
help="Mavlink port baud rate", default=115200)
|
||
|
|
parser.add_argument("--devnum", default=2, type=int,
|
||
|
|
help="PX4 UART device number (defaults to GPS port)")
|
||
|
|
parser.add_argument("--devbaud", default=115200, type=int,
|
||
|
|
help="PX4 UART baud rate (defaults to u-Blox GPS baud)")
|
||
|
|
parser.add_argument("--tcpport", default=2001, type=int,
|
||
|
|
help="local TCP port (defaults to %(default)s)")
|
||
|
|
parser.add_argument("--tcpaddr", default='127.0.0.1', type=str,
|
||
|
|
help="local TCP address (defaults to %(default)s)")
|
||
|
|
parser.add_argument("--debug", default=0, type=int,
|
||
|
|
help="debug level")
|
||
|
|
parser.add_argument("--buffsize", default=128, type=int,
|
||
|
|
help="buffer size")
|
||
|
|
parser.add_argument("--login", default=None, help="input log file")
|
||
|
|
parser.add_argument("--logout", default=None, help="output log file")
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
print("Connecting to MAVLINK...")
|
||
|
|
mav_serialport = mavutil.MavlinkSerialPort(
|
||
|
|
args.mavport, args.mavbaud,
|
||
|
|
devnum=args.devnum, devbaud=args.devbaud, debug=args.debug)
|
||
|
|
|
||
|
|
listen_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||
|
|
listen_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||
|
|
listen_sock.bind((args.tcpaddr, args.tcpport))
|
||
|
|
listen_sock.listen(1)
|
||
|
|
|
||
|
|
print("Waiting for a TCP connection.")
|
||
|
|
print("Use tcp://%s:%d in u-Center." % (args.tcpaddr, args.tcpport))
|
||
|
|
conn_sock, addr = listen_sock.accept()
|
||
|
|
conn_sock.setblocking(0) # non-blocking mode
|
||
|
|
print("TCP connection accepted. Use Ctrl+C to exit.")
|
||
|
|
|
||
|
|
login = None
|
||
|
|
logout = None
|
||
|
|
if args.login:
|
||
|
|
login = open(args.login, "w")
|
||
|
|
if args.logout:
|
||
|
|
logout = open(args.logout, "w")
|
||
|
|
|
||
|
|
while True:
|
||
|
|
try:
|
||
|
|
data = conn_sock.recv(args.buffsize)
|
||
|
|
if data:
|
||
|
|
if login:
|
||
|
|
login.write(data)
|
||
|
|
if args.debug >= 1:
|
||
|
|
print('>', len(data))
|
||
|
|
mav_serialport.write(data)
|
||
|
|
except socket.error:
|
||
|
|
pass
|
||
|
|
|
||
|
|
data = mav_serialport.read(args.buffsize)
|
||
|
|
if data:
|
||
|
|
if logout:
|
||
|
|
logout.write(data)
|
||
|
|
if args.debug >= 1:
|
||
|
|
print('<', len(data))
|
||
|
|
conn_sock.send(data)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main()
|