#!/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()