Andrew Tridgell
6 years ago
2 changed files with 185 additions and 0 deletions
@ -0,0 +1,11 @@
@@ -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 @@
@@ -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