Hi all,
I am trying to autonomously control my Skyviper drone in a map consisting numerous Aruco markers on a plane. I am able to localize my drone by using the aruco detection and pose estimation libraries that OpenCV provides. I have written a ros node that publishes /mavros/vision_position/pose which contains the pose of the drone in NED coordinate system. I have whitelisted vision* and local* plugins in the apm_pluginlist.
Then I switch on my drone and connect my PC to it via WiFi. Next, I run mavros node and pass the correct fcu_url as a parameter. Then I run the ros node which provides the topic /mavros/vision_position/pose to the mavros node. The FCU accepts these vision position estimates and prints the following on the mavros terminal (and also on QGroundcontrol’s terminal):
[ INFO] [1537563174.837272658]: FCU: EKF2 IMU0 is using external nav data
[ INFO] [1537563174.838611490]: FCU: EKF2 IMU0 initial pos NED = 0.0,0.0,0.0 (m)
[ WARN] [1537563174.902127448]: PR: request param #684 timeout, retries left 2, and 2 params still missing
[ INFO] [1537563174.902845720]: FCU: EKF2 IMU0 ext nav yaw alignment complete
Next I set the home position by running another ros node which is given here: https://github.com/vincekurtz/ardupilot_gazebo/blob/master/src/set_origin.py
When the home position is set, the mavros node publishes /mavros/local_position/pose which is more or less close to /mavros/vision_position/pose. Here is a video which shows the visualization of these two topics: https://youtu.be/975btD4QMhA
But when I try to fly the drone using this external navigation data, the drone takes off and then drifts a lot. It gains height and hits the roof even though I have set the take off altitude to just 1.5 m. Then it exhibits really unexpected behavior. Can anyone help me fix this issue? @rmackay9 @vkurtz @anbello