From dbc1cd2b9615507451bef5dc6aec672d3fb73464 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Aug 2019 17:28:47 +1000 Subject: [PATCH] Tools: support GPS yaw in Vicon script --- Tools/Vicon/vicon_mavlink.py | 47 +++++++++++++++++++++++++++++------- 1 file changed, 38 insertions(+), 9 deletions(-) diff --git a/Tools/Vicon/vicon_mavlink.py b/Tools/Vicon/vicon_mavlink.py index 64af3b6886..3ca94c2802 100755 --- a/Tools/Vicon/vicon_mavlink.py +++ b/Tools/Vicon/vicon_mavlink.py @@ -6,6 +6,11 @@ Released under GNU GPL v3 or later from pyvicon import pyvicon from pymavlink.quaternion import Quaternion + +# force mavlink2 for yaw in GPS_INPUT +import os +os.environ['MAVLINK20'] = '1' + from pymavlink import mavutil from pymavlink import mavextra import math @@ -26,6 +31,8 @@ parser.add_argument("--source-component", type=int, default=mavutil.mavlink.MAV_ 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") +parser.add_argument("--gps-only", action='store_true', default=False, help="only send GPS data") +parser.add_argument("--send-zero", action='store_true', default=False, help="send zero data") args = parser.parse_args() @@ -48,13 +55,16 @@ last_origin_send = 0 def connect_to_vicon(ip): '''connect to a vicon with given ip or hostname''' global vicon + print("Opening connection to %s" % ip) vicon.connect(ip) + print("Configuring vicon") 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 + print("waiting for vehicle...") while True: vicon.get_frame() name = vicon.get_subject_name(0) @@ -91,7 +101,10 @@ def main_loop(): gps_period_ms = 1000 // args.gps_rate while True: - vicon.get_frame() + if args.send_zero: + time.sleep(0.05) + else: + vicon.get_frame() now = time.time() now_ms = int(now * 1000) @@ -104,7 +117,10 @@ def main_loop(): last_msg_time = now # get position as ENU mm - pos_enu = vicon.get_segment_global_translation(name, segname) + if args.send_zero: + pos_enu = [0.0, 0.0, 0.0] + else: + pos_enu = vicon.get_segment_global_translation(name, segname) if pos_enu is None: continue @@ -112,11 +128,19 @@ def main_loop(): 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) + if args.send_zero: + quat = [0.0, 0.0, 0.0, 1.0] + else: + 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 + yaw -= math.pi + + yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100) + if yaw_cd == 0: + # the yaw extension to GPS_INPUT uses 0 as no yaw support + yaw_cd = 36000 if args.debug > 0: print("Pose: [%.3f, %.3f, %.3f] [%.2f %.2f %.2f]" % ( @@ -125,7 +149,7 @@ def main_loop(): time_us = int(now * 1.0e6) - if now - last_origin_send > 1: + if now - last_origin_send > 1 and not args.gps_only: # send a heartbeat msg mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0) @@ -146,6 +170,7 @@ def main_loop(): 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 ] + last_gps_pos = pos_ned gps_week, gps_week_ms = get_gps_time(now) @@ -158,15 +183,19 @@ def main_loop(): 1.0, 1.0, gps_vel[0], gps_vel[1], gps_vel[2], 0.2, 1.0, 1.0, - args.gps_nsats) + args.gps_nsats, + yaw_cd) 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) + if not args.gps_only: + # we force mavlink1 to avoid the covariances which seem to make the packets too large + # for the mavesp8266 wifi bridge + mav.mav.global_vision_position_estimate_send(time_us, + pos_ned[0], pos_ned[1], pos_ned[2], + roll, pitch, yaw, force_mavlink1=True) connect_to_vicon("vicon")