EKF2 VIO not working, settings and code not correct?

For using VIO data from a stereo-vision camera, I have done tranformations within ROS1 where I publish the data, if anyone could possibly check the code I have produced here and let me know of any mistakes or changes that should be made would be great. As I am using the ZED 2i, I had to manually calculate the linear and angular velocities within the code.

For the setting on PX4, I’m using v1.14 on the Pixhawk Cube, for the parameter EKF2_EV_CTRL I have set the number to 13, which is a tick for “Horizonatal Position”, “3D Velocity”, and “Yaw”, is this correct? I did not tick “Vertical Position” as I have a downward facing small FOV LiDAR for absolute height above ground, which works pretty well on its own for keeping absolute height.

I did a test flight with this code with only “Horizontal position” and “Yaw” ticked, and the drone crashed head first to the right into a wall (after a while of flying). Does the “3D velocity” part need to be ticked as well?

Also, how important is the EKF2_EV_DELAY parameter? As I have not set this as I don’t know how to check it (docs don’t help).

The end goal, I just want the drone to maintain a stable position in POSITION flight mode. Followed by issuing commands in OFFBOARD flight mode, code which I already have set up.

I tested in the simulation that the UAV can maintain position mode without 3D speed.

very important

could you provide a log file?

Okay, so if doing without 3D speed, then you would have to not publish it to /mavros/odometry/out then to prevent it fusing with EKF2? Or well not ticking it should ensure it’s not fused but sometimes hard to tell.

Okay, was thinking, certainly, here is the log file of my crash that occured (crash log file) and here is another log file where I attemped POSITION flight mode a few times (other log file)

Log shows that the visual data stopped shortly after entering the position mode in the final stage.

How did you get that data to show up like that? So the visual data cut out at the end and that’s why it crashed? I wonder why it cut out…

The visual data doesn’t make the drone maintain position still, do you know any troubleshooting methods or what I might’ve missed?