Problems using set_gps_global_origin and hil_gps mavlink messages


After reading the documentation for and the source code for mavlink_receiver.cpp and the hil_gps and set_gps_global_origin I incorporated them into a program I use to optionally integrate mocap data as odometry and/or synthetic gps and fuse it with a ned converted onboard VIO odometry estimate for a drone with PX4-firmware, but I’m getting unexpected behavior when sending both of the mavlink messages.

Background: I have GPS enabled for only lat/lon fusion in the ekf and I’ve turned off the driver for the physical gps sensor when flying indoors so PX4 only starts getting gps sensor data when I start sending mocap data converted to into gps via the hil_gps message (hil_gps enabled in px4 params). I have all other EKF control voided except for external vision. When I start the drone up PX4 should basically only be using IMU data until I run my mocap server and then it will start receiving onboard VIO generated odometry data in the local ned frame and optionally mocap as local ned frame odometry and/or mocap as synthetic gps. A set_gps_global_origin is sent before any of the mocap data is let through and it shows up as being properly set in the ulog file after the fact. Here are the problems I’ve encountered:

  1. set_gps_global_origin appears to work because I get this in the log:
    [muorb] SLPI: 0 - New NED origin (LLA): x, y, z

where x,y,z are the valid lat, lon, alt, but it doesn’t actually work when I’m only sending external vision data in the form of odometry. Local position estimates are updated, but global_position_estimates were never broadcast, nor was a valid gps_global_origin. I only seem to get valid global positions and gps_global_origins when I concurrently stream mocap as synthetic gps data for a prolonged period of time.

  1. When I do send odometry data for local position estimation and synthetic gps data for global position, it throws the ekf out of ev yaw control whenever it starts receiving the fake gps data and I’m showing constant yaw estimates errors in QGC. Not sure if this because mavlink_receiver sets heading from the sensor data for hil_gps as null or not, but I don’t understand why the ekf is using any of the gps data for yaw fusion if I have it set not to with px4 params. Also if the mavlink message for hil_gps has a field for yaw, why doesn’t mavlink receiver parse it?

Not sure if any of this is a timing issue or not. Do I need to let a stream of EV data come in before I set the gps_global_position to get a valid global origin and global position estimates? How do I prevent hil_gps from throwing me out of EV yaw control? I’m trying to avoid making any changes to the firmware

Any troubleshooting help if appreciated. Thanks!