Alright, solved it. I adjusted the MCS pose rate to be at 30 Hz (should be fine up to 50 Hz according to PX4 documentation), although this does not seem to have any significant impact.

The problem was that the EKF2_AID_MASK must be set to 24 (only position and yaw fusion) for motion capture, instead of 280 (default with VIO, which sends odometry messages including velocities). Hence the following fields must be set as follows:

I did not have to modify any other parameters.

@James-Strawson, thanks again for the help!

I just realized the MCS pose publishing rate is likely too high, which may cause the lag. I am investigating...

Hi @James-Strawson,

Thanks a lot for the very detailed answer, much appreciated!

I finally managed to have PX4 receive Motion Capture System pose data and fuse it. Here is what I did (following your guidelines):

• I disabled the qvio server with: systemctl disable voxl-qvio-server
• I installed from source the mavros_extras package (v1.0.0) and enabled the vision_pose_estimate plugin by publishing on the topic /mavros/vision_pose/pose (of type geometry_msgs::PoseStamped).
• A node listening for the MCS poses runs onboard VOXL and stamps the poses with the monotonic clock.

When I visualize the MCS pose and the /mavros/local_position/pose (PX4 state estimate) in RVIZ while manually flying the drone, the estimate lags behind the MCS pose. It sometimes loses track of the MCS pose and jumps back to realign with it. Also, QGroundControl sometimes relays that the sensor calibration should be checked.

Any suggestion on where this behavior might come from?

What about publishing MCS pose messages of type vio_data_t on the qvio pipe to leverage the MPA?

Hi,

I am trying to send Motion Capture System (MCS) data to the FCU to fuse with or replace the VIO estimate. I have been reading some posts about this topic as I am not the first one tackling this, but there does not seem to be an accepted way of doing it.

I run a node onboard the kinetic docker image onboard voxl that subscribes to our MCS and generates a PoseStamped ROS message. I compiled the vision_pose_estimate plugin from mavros_extras as an independent node to relay the pose messages over mavlink, and configured it as follows (according to the way mavros node is launched):

int 		system_id 	= 1; // env PX4_SYS_ID
int 		component_id	= mavconn::MAV_COMP_ID_UDP_BRIDGE; // 240 
int 		tgt_system_id	= 1; // env PX4_SYS_ID
int 		tgt_component_id= 1; // env PX4_COMP_ID
std::string fcu_protcol		= "v2.0";
bool		respawn_mavros	= false;

Since the file /etc/modalai/voxl-vision-px4.conf is set with:

"qgc_udp_port_number": 14550,
"localhost_udp_port_number": 14551,

I let the vision_pose_estimate node send data over UDP via port 14552 (sending to 14551 blocks any other communication). The node seems to be relaying pose data fine (at least no error when sending the data). However, I am not sure whether PX4 is actually seeing any of it... I tried looking at the mavlink shell provided by voxl-px4-shell and ran the command listener vehicle_visual_odometry, but only the VIO data seems to be received.
How can I check whether the MCS pose is reaching PX4?

@Chad-Sweet mentioned that modifying the voxl-vision-hub at this location would allow relaying MCS pose instead of VIO pose.
Is it indeed the recommended way?

Thanks in advance for any help!

The voxl-version command returns the following, running of VOXL-Flight:

voxl-version 
cat: /etc/modalai/voxl-software-bundle-version.txt: No such file or directory
--------------------------------------------------------------------------------
system-image:    ModalAI 3.3.0 BUILDER: ekatzfey BUILD_TIME: 2021-06-06_19:28
kernel:          #1 SMP PREEMPT Sun Jun 6 19:41:01 UTC 2021 3.18.71-perf
factory-bundle:  1.0.1 (Yocto installation)
--------------------------------------------------------------------------------
architecture:    aarch64
processor:       apq8096
os:              GNU/Linux
--------------------------------------------------------------------------------
voxl-suite:
Package: voxl-suite
Version: 0.5.0
Depends: libjpeg_turbo (>= 9.0.4), libmodal_exposure (>= 0.0.2), libmodal_json (>= 0.3.6), libmodal_pipe (>= 2.1.1), librc_math (>= 1.1.5), libvoxl_cutils (>= 0.0.2), libvoxl_io (>= 0.5.4), mavlink-camera-manager (>= 0.0.2), mongoose (>= 7.3.0), opencv (>= 4.5.2-2), openmp (>= 10.0.2), voxl-camera-calibration (>= 0.1.1), voxl-camera-server (>= 0.8.1), voxl-cpu-monitor (>= 0.2.0), voxl-dfs-server (>= 0.2.2), voxl-docker-support (>= 1.1.3), voxl-gphoto2 (>= 0.0.5), voxl-imu-server (>= 0.9.1), voxl-mavlink (>= 0.0.2), voxl-modem (>= 0.12.0), voxl-mpa-tools (>= 0.3.6), voxl-nodes (>= 0.2.0), voxl-portal (>= 0.1.2), voxl-qvio-server (>= 0.3.4), voxl-streamer (>= 0.2.6), voxl-tag-detector (>= 0.0.2), voxl-tflite (>= 2.2.3), voxl-tflite-server (>= 0.1.5), voxl-utils (>= 0.8.4), voxl-vision-px4 (>= 0.9.5), voxl-vpn (>= 0.0.3)
Status: install user installed
Section: base
Architecture: all
Maintainer: james@modalai.com
MD5Sum: f55ec020942e92ff12bc137c2aa8d2a5
Size: 1956
Filename: voxl-suite_0.5.0.ipk
Description: meta-package for voxl-suite software collection
Installed-Time: 1642294777

Package: voxl-suite
Version: 0.2.0
Depends: docker, imu_app (= 0.0.6), libvoxl_io (= 0.5.2), voxl-cam-manager (= 0.2.2), voxl-docker-support (= 1.1.1), voxl-hal3-tof-cam-ros (= 0.0.2), voxl-modem (= 0.10.0), voxl-nodes (= 0.0.8), voxl-rtsp (= 1.0.2), voxl-utils (= 0.5.2), voxl-vision-px4 (= 0.6.8), voxl_imu (= 0.0.4), voxl-time-sync (= 0.0.1), voxl-vpn (= 0.0.2), librc_math (= 1.1.2), libmodal_pipe (= 1.2.2), modalai-vl (= 0.1.3)
Status: unknown ok not-installed
Section: base
Architecture: armv7a
Maintainer: james@modalai.com
MD5Sum: af706cd3c1ea59f274f2ed9b93141f1d
Size: 870
Filename: voxl-suite_0.2.0.ipk
Description: meta-package to install all of the voxl-suite

Package: voxl-suite
Version: 0.4.6
Depends: libmodal_exposure (>= 0.0.2), libmodal_json (>= 0.3.5), librc_math (>= 1.1.5), libvoxl_cutils (>= 0.0.2), libvoxl_io (>= 0.5.4), opencv (>= 4.5.2-1), voxl-mavlink (>= 0.0.2), libmodal_pipe (>= 2.0.7), voxl-camera-server (>= 0.7.1), voxl-cpu-monitor (>= 0.1.7), voxl-dfs-server (>= 0.2.0), voxl-imu-server (>= 0.8.1), voxl-mpa-tools (>= 0.2.6), voxl-qvio-server (>= 0.3.1), voxl-tag-detector (>= 0.0.2), voxl-tflite-server (>= 0.1.0), voxl-vision-px4 (>= 0.9.2), openmp (>= 10.0.2), voxl-tflite (>= 0.0.1), voxl-nodes (>= 0.1.6), mavlink-camera-manager (>= 0.0.2), voxl-docker-support (>= 1.1.3), voxl-gphoto2 (>= 0.0.5), voxl-modem (>= 0.12.0), voxl-streamer (>= 0.2.3), voxl-utils (>= 0.7.1), voxl-vpn (>= 0.0.3)
Replaces: voxl-suite-dev
Status: unknown ok not-installed
Section: base
Architecture: all
Maintainer: james@modalai.com
MD5Sum: a800fd5f7c98cfa4e6ed1638cf25d909
Size: 1902
Filename: voxl-suite_0.4.6.ipk
Description: meta-package for voxl-suite software collection

--------------------------------------------------------------------------------

@kiprock Thank you for this, that also solved the issue in my case.

To be a little more cautious, I simply renamed the existing file, which forced the generation of a new one when running the configuration script voxl-configure-docker-support.sh:

voxl:/data/network/files$ mv local-kv.db local-kv_DISABLED.db

I do not think I have access to this slack. It seems restricted to a few specific people only. But I tried their forum. Thanks anyway!

I would like to send altitude data produced by embedded, downward-looking range finders (VL53l1X and teraranger evo 60m) to the auto-pilot. I would like to do it over mavros since I already have interfaced the sensors over ROS (connected to voxl I²C port 7).

Should I try to use the /mavros/vision_pose/pose topic? There are also several PX4 parameters (EKF2_RNG_*) that have to be set (defining when to use the range finders, e.g. at takeoff and while landing), but I am not sure if those parameters are supposed to work only with supported flight controller hardware when connecting specific range finders directly to them.

Long story short, I haven’t found a clear way of publishing altitude data over mavros to let the EKF2 fuse this information into the state estimate. Any hint?