mirror of https://github.com/ArduPilot/ardupilot
Tools: added Vicon mavlink gateway script
This commit is contained in:
Andrew Tridgell
parent
142a4a105a
commit
562f4ed1a3
|
|
@ -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.
|
||||
|
|
@ -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()
|
||||
|
||||
Loading…
Reference in New Issue