I have recently been attempting to integrate a motion capture (MoCap) system as an external navigation source using MAVROS; however, I am running into some difficulties getting the fused sensor data (LOCAL_POSITION_NED) from the EKF. The problem that I am observing is that, when I launch my localization framework while QGC is open, I do not receive any LOCAL_POSITION_NED messages. I was initially under the impression that QGC was adjusting the publishing rates, but I already integrate the solution applied by @clyde in Orca4 here. When I launch my framework without QGC open, I start to get these messages (albeit with the log IMU0 stopped aiding). Here are some of the debugging steps that I have already tried and their outcomes:
Everything that I have implemented works quite well in my ROS 2 Gazebo SITL environment; however, problems start to pop up in the hardware integration (sim2real issues are basically a fact of life ).
When I launch MAVROS, I am able to successfully connect to the BlueROV. I am able to establish a connection both from the topside (my laptop) and from a Docker container on the BlueROV RPi itself. MAVROS is also able to establish a connection with QGC in both scenarios. When using the topside interface, I use the same connection string that @patrickelectric applied here.
I have verified that the VISION_POSITION_ESTIMATE message is being received using the BlueOS MAVLink inspector
I tried using the DVL BlueOS extension with our DVL (without my localization framework launched) and I am able to both receive the LOCAL_POSITION_NED messages over MAVROS and set the message interval, as previously mentioned, all with QGC open.
I have verified that my external state interface is working properly. I have applied an LWMA filter to the MoCap data and also filter out any invalid NaN data. This was in an attempt to handle the IMU0 stopped aiding message and to account for the possibility that the EKF is rejecting my measurements. I also checked the LOCAL_POSITION_NED messages that are published when my framework is running with QGC closed and have confirmed that these messages are accurate.
I set the EKF origin using the global_position MAVROS plugin. I have confirmed that the location is set when I open QGC and see the ROV icon at the specified location.
I set the ArduSub parameter values to the same values as those applied by the DVL driver in BlueOS
I was originally publishing the MoCap data at a rate of 100 Hz, but observed some slowdown in the system response at this frequency. I originally hypothesized that the CPU was operating at its limits, but saw that the CPU usage was only at about ~50% in BlueOS. I don’t believe that the BlueOS core container is throttling anything either. I dropped the MoCap streaming rate down to about 50 Hz and the response rate improved, but the main issue persisted.
I am using ArduSub 4.1.0 Stable. I was originally using the Dev version, but observed some issues with the DVL driver integration, and this version isn’t backwards compatible with the latest version of QGC
Finally, I tried integrating an estimated covariance measurement (I just set it to a low 0.1 value because the MoCap has ~2mm accuracy at the time of calibration)
Given these things, I believe that the issue is on my end; however, I am a bit unsure where to go from here and was hoping to look for some feedback and also see if there might be something that the DVL driver needed to do to get the integration working. Below are some of the relevant links within my implementation:
Here is where I process the published MoCap data and send it to the FCU
Did you mean VISION_POSITION_ESTIMATE? Or some other message?
Are you getting the “ekf3 imu0 started relative aiding” and “ekf3 imu0 fusing odometry” messages in the GCS?
Can you share the relevant parameters?
I’ve never gotten this to work IRL (just simulation), but looking at libraries/GCS_MAVLink/GCS_Common.cpp it seems that it should work w/ VISION_POSITION_ESTIMATE, GLOBAL_VISION_POSITION_ESTIMATE, VISION_POSITION_DELTA, VISION_SPEED_ESTIMATE and/or several other messages. It might be worth tracing through this code and adding printfs to see if NavEKF3_PosVelFusion is getting called correctly.
The DVL extension sends these messages, so presumably this combination will work:
VISION_POSITION_DELTA
VISION_SPEED_ESTIMATE
GLOBAL_VISION_POSITION_ESTIMATE
SET_GPS_GLOBAL_ORIGIN
Copter also has some tests that might be insightful.
You might have some luck posting this on the ardupilot forums since there are a lot of copter folks who do this sort of thing.
Thank you for the reply! Yes I have updated the post accordingly. Thanks!
To answer your other questions, I don’t receive those specific messages, but those that I do receive are consistent with what I get in simulation:
[mavros_node-1] [INFO] [1692492986.750783342] [mavros.sys]: FCU: EKF3 IMU0 initialised
[mavros_node-1] [INFO] [1692492986.753117693] [mavros.sys]: FCU: AHRS: EKF3 active
[mavros_node-1] [INFO] [1692492988.418222143] [mavros.sys]: FCU: EKF3 IMU0 tilt alignment complete
[mavros_node-1] [INFO] [1692492988.428364122] [mavros.sys]: FCU: EKF3 IMU0 yaw aligned
[mavros_node-1] [INFO] [1692492988.430411247] [mavros.sys]: FCU: EKF3 IMU0 is using external nav data
[mavros_node-1] [INFO] [1692492988.430502789] [mavros.sys]: FCU: EKF3 IMU0 initial pos NED = -0.0,-0.0,0.3 (m)
When QGC is closed and I am receiving messages, I get something along the lines of
FCU: EKF3 IMU0 is using external nav data
FCU: EKF3 IMU0 initial pos NED = -0.0,-0.0,0.3 (m)
FCU: EKF3 IMU0 stopped aiding
where the initial pos gets updated with the current vision estimate, and is repeated for each message. I am hoping to grab a log Monday morning to provide some additional context. I will grab the parameters that I have set as well (I forgot to grab these on my way out of the lab on Friday), though the parameters that are set are consistent with these.
I haven’t tried is sending a GLOBAL_VISION_POSITION_ESTIMATE. I am curious why the DVL driver uses that rather than the VISION_POSITION_ESTIMATE. It appears that some of the ArduPilot Vicon tools also use this, but, simultaneously, it looks like the GCS Common library handles both messages the same way here. An alternative that I may try is the MoCap plugin from MAVROS, which I believe gets handled by a different interface than the *VISION_POSITION_ESTIMATE messages in the GCS Common library here. I have also seen some discussion mentioning that the home position needs to be set, though I haven’t seen anything in the DVL driver to indicate this. I did make an attempt to set the home position on the vehicle, but the service request was rejected. My guess is that QGC is setting the home position to my location which I didn’t try disabling at the time. I will likely make another attempt to set that.
I made an additional discussion per your recommendation on the ArduPilot forums here.
AFAICT only the VISION_POSITION_DELTA is sent by the DVL extension. The other messages may work as well, but they have not been tested by the DVL extension.
).