EKF2 not working with mocap data on pixhawk 4

#1

We are working on Pixhawk 4 with the following specifications:
HW arch: PX4FMU_V5
Firmware version: 1.8.2

We are trying to use vicon for external position estimation and our pixhawk uses EKF2 as the estimator.
We are using following parameters for enabling external position estimation(using QGCS):
EKF2_AID_MASK: 24
EKF2_HGT_MODE: 3
EKF2_EV_DELAY: 0
We are using odroid as companion computer (where we run mavros using /dev/ttyACM99 and 57600 as baud rate), a laptop running rosmaster to get the vicon messages which is connected to odroid over wifi, and we connect the pixhawk to the odroid using telem2. We use mavproxy to access nsh shell on odroid using USB port of Pixhawk.

We are subscribing to our mocap_vicon ROS node to get position data and are relaying it on /mavros/vision_pose/pose topic using topic_tools ROS command. On doing rostopic echo on /mavros/vision_pose/pose topic, we are able to see the messages but it seems Pixhawk is not receiving these messages and therefore it is not using our vicon position for position estimation. We also tried to write a ROS node subscribing to vicon node and publishing on /mavros/vision_pose/pose but still facing the same problem.
We verified the error by logging into pixhawk and using its nsh shell to echo uorb topics. /mavros/vision_pose/pose is mapped to vehicle_vision_position uorb topic but when we echo it, it says topic never published. Also, we checked estimator_status variable. The value of its solution_status_flags is 164 with its 7th bit set which saya “True if the EKF is in a constant position mode and is not using external measurements” which means no external position estimates are being used.

Here is the log file:

#2

Hello,
Were you able to solve this issue or still trying to fix it? I have a same problem. I am able to get into offboard mode but I doubt if px4 is getting the /mavros/vision_pose/pose messages. Also,
If I try to put my quadcopter in position hold mode, the logs show that the mode is rejected. Thank you.

#3

Hello @arg1885, I have the same exact issue, but in my case I use the OptiTrack motion capture readings to encode the SET_POSITION_LOCAL_TARGET_NED. I am not sure if the encoded message is received by my px4 mini 4 controller. when I put the same to offboard it never takes off it just sits at its origin. Did you check MAVLink inspector stream if position NED is set via Qground ?

#4

Hello. I tried few things yesterday while following this topic given here. https://docs.px4.io/en/advanced_config/tuning_the_ecl_ekf.html

https://dev.px4.io/en/ros/external_position_estimation.html#relaying_pose_data_to_px4

Let me try and ensure that the /mavros/local_position/pose is matching the mocap data. Meanwhile let me know if you are able to make it work. Thank you.

#5

Hello @arg1885 , i collected .ulog file from controller and extracted the csv file for ekf setpoints, but from which i do not see the exact data that i fed via mocap. I’m now not sure as what to debug next ?. If any one has relative pointers and suggestion for this issue please do let us know.

From above experiment I confirmed that data read from mavlink inspector and ulog files point to same issue where NED position fed from mocap ARE NOT reflecting/read by pixhawk. Attaching log files references screen prints

Below is the message format encoding used to set NED position over mavlink:
SET_POSITION_TARGET_LOCAL_NED {time_boot_ms : 1521387039, target_system : 0, target_component : 0, coordinate_frame : 1, type_mask : 4088, x : 0.1243, y : 0.17761, z : 0.4003, vx : 0, vy : 0, vz : 0, afx : 0, afy : 0, afz : 0, yaw : 0, yaw_rate : 0}

Thanks,
scalnoor