# Send Motion Capture System pose data to PX4 Source: https://forum.modalai.com/topic/2118/send-motion-capture-system-pose-data-to-px4 Category: General Questions (https://forum.modalai.com/category/2/general-questions) Posted: 2023-05-02 12:53:02 UTC by LucasW Replies: 5 · Views: 4584 ## LucasW · 2023-05-02 12:53:02 UTC 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](https://github.com/mavlink/mavros/blob/ros2/mavros_extras/src/plugins/vision_pose_estimate.cpp) 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): ```cpp 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: ```txt "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](https://gitlab.com/voxl-public/voxl-sdk/services/voxl-vision-hub) at this [location](https://gitlab.com/voxl-public/voxl-sdk/services/voxl-vision-hub/-/blob/master/src/vio_manager.c#L199) 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:* ```txt 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 -------------------------------------------------------------------------------- ``` ## Reply by LucasW · 2023-05-03 15:08:41 UTC What about publishing MCS pose messages of type [vio_data_t](https://gitlab.com/voxl-public/voxl-sdk/core-libs/libmodal-pipe/-/blob/master/library/include/modal_pipe_interfaces.h#L849) on the qvio pipe to leverage the [MPA](https://docs.modalai.com/mpa/)? ## Reply by James Strawson (ModalAI staff) · 2023-05-03 17:18:44 UTC Hi Lucas, To do this through mavros: 1. make sure voxl-qvio-server is disabled 2. disable the vio thread in voxl-vision-hub by setting en_vio = false in /etc/modalai/voxl-vision-hub.conf (used to be called /etc/modalai/voxl-vision-px4.conf on SDK 0.9 and older) 3. Point mavros to connect to port 14551 which is opened by voxl-vision-hub for talking to mavros/mavsdk. This is the port I use to send in target_local_position_ned commands to do a figure-eight demo with mavros. 4. Make sure that you configure your MCS node and mavros to send in the odometry messages as accurately timestamped to the linux monotonic clock as possible so PX4's EKF2 can account for the pose estimation and transmission delay. - In the case of an external flight controller connected to VOXL, voxl-vision-px4 (SDK0.9 and older) or voxl-mavlink-server (SDK1.0 and newer) respond to PX4's timesync requests so that PX4 has a good estimate of the clock delta between PX4 and Linux Clock Monotonic. - In the case of PX4 running onboard a VOXL2, PX4 (and therefore EKF2) natively uses the linux monotonic clock so they are perfectly in sync. - I have seen people's ROS nodes send out monotonic time until VOXL can ping an NTP server, then ROS suddenly sends timestamps in clock-realtime. This jump is unacceptable, all sensor data packets in the entire system need to share one consistent monotonic clock with no discontinuities. 5. PX4 will report its own odometry message as well as a local_position_ned message with mostly similar data. If your MCS data is going through then PX4's reported odometry should track yours closely. You can also do this by publishing a pipe. You don't have to copy the qvio pipe name, in SDK0.9 you can edit the "vio_pipe" field in /etc/modalai/voxl-vision-px4.conf to point to your own pipe. I've seen others struggle with MAVROS messing with their timestamps. Port 14551 is a straight shot through from MAVROS to PX4, the only messages that are touched are local position setpoint messages and that's only when using the apriltag relocalization feature in voxl-vision-hub (formerly voxl-vision-px4). Otherwise whatever MAVROS sends to that port goes straight to PX4 unaltered. I would suggest starting there and if PX4 still rejects the MAVROS messages then try publishing into MPA with the vio_data_t type Good Luck, James ## Reply by LucasW · 2023-05-10 16:17:42 UTC 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? ## Reply by LucasW · 2023-05-10 17:30:40 UTC I just realized the MCS pose publishing rate is likely too high, which may cause the lag. I am investigating... ## Reply by LucasW · 2023-05-11 09:50:48 UTC **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: --- [ ] ... [x] `vision position fusion` [x] `vision yaw fusion` [ ] ... [ ] `vision velocity fusion` **<-- Disable for Motion Capture** (enabled by default for VIO) --- I did not have to modify any other parameters. @James-Strawson, thanks again for the help!