Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tools: added Vicon mavlink gateway script

This commit is contained in:
Andrew Tridgell 2019-08-02 17:56:47 +10:00
parent 142a4a105a
commit 562f4ed1a3
2 changed files with 185 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Tools/Vicon/README.md Normal file
View File

@ -0,0 +1,11 @@
# Vicon MAVLink Gateway
This script produces GLOBAL_VISION_POSITION_ESTIMATE, GPS_INPUT and
GPS_GLOBAL_ORIGIN messages based on data from a Vicon indoor
positioning system.
It uses the pyvicon library from here:
https://github.com/MathGaron/pyvicon
which is a python wrapper around the Vicon SDK.

174
Tools/Vicon/vicon_mavlink.py Executable file
View File

@ -0,0 +1,174 @@
#!/usr/bin/env python3
'''
connect to a vicon and send mavlink vision data on UDP
Released under GNU GPL v3 or later
'''
from pyvicon import pyvicon
from pymavlink.quaternion import Quaternion
from pymavlink import mavutil
from pymavlink import mavextra
import math
import time
import sys
from argparse import ArgumentParser
parser = ArgumentParser(description="vicon to mavlink gateway")
parser.add_argument("--vicon-host", type=str,
help="vicon host name or IP", default="vicon")
parser.add_argument("--mavlink-dest", type=str, help="mavlink destination", default="udpout:127.0.0.1:14550")
parser.add_argument("--origin", type=str, help="origin in lat,lon,alt,yaw", default="-35.363261,149.165230,584,270")
parser.add_argument("--debug", type=int, default=0, help="debug level")
parser.add_argument("--target-sysid", type=int, default=1, help="target mavlink sysid")
parser.add_argument("--source-sysid", type=int, default=255, help="mavlink source sysid")
parser.add_argument("--source-component", type=int, default=mavutil.mavlink.MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY, help="mavlink source component")
parser.add_argument("--rate", type=float, default=None, help="message rate for GLOBAL_VISION_POSITION_ESTIMATE")
parser.add_argument("--gps-rate", type=int, default=5, help="message rate for GPS_INPUT")
parser.add_argument("--gps-nsats", type=int, default=16, help="GPS satellite count")
args = parser.parse_args()
# create a mavlink serial instance
mav = mavutil.mavlink_connection(args.mavlink_dest,
source_system=args.source_sysid,
source_component=args.source_component)
# create vicon connection
vicon = pyvicon.PyVicon()
origin_parts = args.origin.split(',')
if len(origin_parts) != 4:
print("Origin should be lat,lon,alt,yaw")
sys.exit(1)
origin = (float(origin_parts[0]), float(origin_parts[1]), float(origin_parts[2]))
last_origin_send = 0
def connect_to_vicon(ip):
'''connect to a vicon with given ip or hostname'''
global vicon
vicon.connect(ip)
vicon.set_stream_mode(pyvicon.StreamMode.ClientPull)
vicon.enable_marker_data()
vicon.enable_segment_data()
vicon.enable_unlabeled_marker_data()
vicon.enable_device_data()
# wait for first subject to appear
while True:
vicon.get_frame()
name = vicon.get_subject_name(0)
if name is not None:
break
print("Connected to subject %s" % name)
def get_gps_time(tnow):
'''return gps_week and gps_week_ms for current time'''
leapseconds = 18
SEC_PER_WEEK = 7 * 86400
MSEC_PER_WEEK = SEC_PER_WEEK * 1000
epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - leapseconds
epoch_seconds = int(tnow - epoch)
week = int(epoch_seconds) // SEC_PER_WEEK
t_ms = int(tnow * 1000) % 1000
week_ms = (epoch_seconds % SEC_PER_WEEK) * 1000 + ((t_ms//200) * 200)
return week, week_ms
def main_loop():
'''loop getting vicon data'''
global vicon
name = vicon.get_subject_name(0)
segname = vicon.get_subject_root_segment_name(name)
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
last_origin_send = now
now_ms = int(now * 1000)
last_gps_send_ms = now_ms
last_gps_send = now
gps_period_ms = 1000 // args.gps_rate
while True:
vicon.get_frame()
now = time.time()
now_ms = int(now * 1000)
if args.rate is not None:
dt = now - last_msg_time
if dt < 1.0 / args.rate:
continue
last_msg_time = now
# get position as ENU mm
pos_enu = vicon.get_segment_global_translation(name, segname)
if pos_enu is None:
continue
# convert to NED meters
pos_ned = [pos_enu[1]*0.001, pos_enu[0]*0.001, -pos_enu[2]*0.001]
# get orientation in euler, converting to ArduPilot conventions
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw -= math.pi*0.5
if args.debug > 0:
print("Pose: [%.3f, %.3f, %.3f] [%.2f %.2f %.2f]" % (
pos_ned[0], pos_ned[1], pos_ned[2],
math.degrees(roll), math.degrees(pitch), math.degrees(yaw)))
time_us = int(now * 1.0e6)
if now - last_origin_send > 1:
# send a heartbeat msg
mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0)
# send origin at 1Hz
mav.mav.set_gps_global_origin_send(args.target_sysid,
int(origin[0]*1.0e7), int(origin[1]*1.0e7), int(origin[2]*1.0e3),
time_us)
last_origin_send = now
if args.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(origin[0], origin[1], pos_ned[0], pos_ned[1])
gps_alt = origin[2] - pos_ned[2]
gps_dt = now - last_gps_send
gps_vel = [ (pos_ned[0]-last_gps_pos[0])/gps_dt,
(pos_ned[1]-last_gps_pos[1])/gps_dt,
(pos_ned[2]-last_gps_pos[2])/gps_dt ]
gps_week, gps_week_ms = get_gps_time(now)
if args.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week, fix_type,
int(gps_lat*1.0e7), int(gps_lon*1.0e7), gps_alt,
1.0, 1.0,
gps_vel[0], gps_vel[1], gps_vel[2],
0.2, 1.0, 1.0,
args.gps_nsats)
last_gps_send_ms = (now_ms//gps_period_ms) * gps_period_ms
last_gps_send = now
# send VISION_POSITION_ESTIMATE
mav.mav.global_vision_position_estimate_send(time_us,
pos_ned[0], pos_ned[1], pos_ned[2],
roll, pitch, yaw)
connect_to_vicon("vicon")
main_loop()