Hello!
I am working on getting a source of external SLAM position estimates forwarded into the sub so that we can utilize GUIDED mode’s velocity and position set points. While the position information is being streamed in to the sub, GUIDED and POSHOLD mode will not initialize because the position_ok() function call returns false. Does the OPTFLOW preprocessor variable need to be enabled to utilize information from VISION_POSITION_ESTIMATE? If not, ideas about directions to look for a solution to this problem would be much appreciated! Also, some intuition about what the values in the flags object in the ekf_position_ok() mean would be very helpful. If this is the wrong place to ask such a question, where should I post this? Below, I detail our setup.
I followed the directions from this blog post Integration of ArduPilot and VIO tracking camera (Part 2): Complete installation and indoor non-GPS flights - Blog - ArduPilot Discourse and have utilized the configuration variables they suggest in the post.
I have a script which passes pose estimates through mavros using the “/mavros/vision_pose/pose” topic at a rate of 30 frames per second. We also have a node which sends a home position and a GPS global origin at a rate of 1 frame per second through the topics “/mavros/global_position/set_gp_origin” and “/mavros/home_position/set”.
Currently, we are passing in information about the sub’s pose relative to an ar tag as the x,y,r,p,y components of the sub position. We take the z-value directly from the “altitude” reading from “/mavros/vfr_hud”. The mavlink console reports that external nav data makes it in:
[ INFO] [1562075062.143843420]: FCU: EKF2 IMU0 is using external nav data
[ INFO] [1562075062.145462097]: FCU: EKF2 IMU0 initial pos NED = 0.5,1.1,0.0 (m)
[ INFO] [1562075062.147514549]: FCU: EKF2 IMU1 is using external nav data
[ INFO] [1562075062.149179114]: FCU: EKF2 IMU1 initial pos NED = 0.5,1.1,0.0 (m)
I am confident that the position estimates are making it through to the sub for two reasons. First, ALT_HOLD mode successfully maintains a roll, pitch, yaw and altitude with the estimated positions. Second, the sub’s altitude reading from “/mavros/vfr_hud” will show anomalous readings in the “altitude” field when the ar tag is first seen. It will enter some kind of feedback loop where the altitude increases indefinitely. Moving the sub around manually (with a pair of tongs) causes the altitude to eventually settle to an accurate value. When the altitude reading is wrong, ALT_HOLD mode reacts properly.
I added print statements in the position_ok() ardupilot/system.cpp at ArduSub-beta · ArduPilot/ardupilot · GitHub function and found that the functions optflow_position_ok() and ekf_position_ok() both return false. I looked further into the call stack and found that optflow_position_ok() fails at line 270 and ekf_position_ok() fails at line 263 or line 259.
To build the code, I followed the directions on the ardusub documentation. First: ./waf configure --board Pixhawk 1, then ./waf build sub. Then, I upload the ardusub.apj file through the web interface. We are currently running the Ardusub-beta tag from the ardupilot github repo.
To add some more data here, I took a look at the fused location data coming from /mavros/local_position/pose and saw that it was totally different from what the external position estimates said with considerable drift in the x,y values over time. Recalibrating the imu through QGC did not fix the problem. When I modified the bitmask for the active imus to exclude all imus, values ceased to be published to the mavros topic.