DVL A50 integration with GPS position

Hey guys
I am wondering what exactly is happening in the background when I click on the global map to set the position of the ROV when using the DVL. I want to use a real GPS reading to update that position instead of clicking on map. Is this a viable option?

Regards

Hi @dbhowmick,

Here’s the relevant code function - it does two different things:

  1. If the positioning system is not initialised it sets the global origin for the vehicle’s position estimates
    • This allows the vehicle’s position estimate to maintain high precision by only storing small number offsets from the configured origin, instead of having to store large numbers that have small variations
  2. If there is an established origin, it just sends the specified position to the autopilot as a new position estimate

There’s been some previous discussion about surface GPS integration, but there hasn’t yet been a definitively established best/stable way of handling it.

The options that come to mind are:

  1. Connecting a GPS to the autopilot and then using a Lua script to selectively enable it at the surface
    • The GPS connection could either be wired with a serial connection to the flight controller board, or using the BlueOS NMEA injector to provide positions from an NMEA GPS
  2. Modifying ArduSub to have a “surface GPS” mode that handles that switching by itself
    • This could yield nice results, but could be somewhat complex to implement, and would require custom firmware builds until the feature gets merged into the main codebase
  3. Connecting a GPS to your own BlueOS Extension (or modifying the Water Linked DVL extension to accept a GPS input), and using that to send its position estimates to the autopilot via visual odometry messages (like the existing “set position” functionality), thus pretending it’s part of the DVL
    • This would avoid needing to change parameters, but may also cause issues if the readings are inconsistent with the DVL, because the autopilot doesn’t know that there are actually two different devices involved
1 Like

we got it working by using the serial bridge to send the gps messages to UDP port and then a python script looks for quality 4 for RTK fix and if fix is found then it updates the origin as you told us how to do that!
another question is how can I also get the rov to use this for navigation if fix is 4. I want the ROV to use the GPS for nav on the surface and DVL below surface. Any ideas?
Also how does the nmea injector work, what parameters do i need to change to make it listen to GPS for nav instead of the DVL
Regards

That’s what the whole second half of my previous comment was about - I’d suggest reviewing that, and checking the links I provided.

The Lua script example I linked to approaches this by configuring two separate Extended Kalman filters (for fusing the sensor readings), which it then switches between[1] depending on whether the GPS signal should be used or ignored.

In that example,

  1. EK3_SRCn_VELXY is consistently set to ExternalNav (6)
    • because the DVL’s velocity estimates are assumed to occur at a higher frequency and precision than the GPS’s updates, but
  2. EK3_SRCn_POSXY is configured as
    1. GPS (3) for SRC1, for use when the GPS signal is available and high quality, and
    2. ExternalNav (6) for SRC2, for when the DVL should be used instead (because the GPS is unavailable / poor quality).

  1. I’m not sure this is possible from outside the autopilot (i.e. I don’t believe there’s a mechanism for this using MAVLink, so this approach requires custom firmware or a Lua script), although if you’re using VISION_POSITION messages for your GPS data instead of GPS_INPUT messages like option 3 from my original comment then I suppose there would be no switching that needs to occur, and the “switch” would just come from stopping sending the position estimates from the GPS. ↩︎

1 Like

As we work through this exact challenge (getting Surface GPS + DVL to read location) I’d love an update on this thread. For instance, is a Lua script still required at this point?

In our situation we have a uBlox M9N GNSS connected to the Navigator + Waterlinked A50 DVL. We have EK3_SRC1_POSXY = GPS and EK3_SRC2_POSXY=ExternalNav.

It all sort of works, i.e. we can pull a track of lat, long. However, one thing we’ve noticed is the depth reading in Cockpit reads incorrectly… We have EK3_SRC1_POSZ = Baro but it’s getting some weird value when the GPS is enabled.

Will do a bit more digging, but any update here appreciated!

Just a clarification on this piece: I ask because we aren’t switching between EK3 source sets and yet we’re getting meaningful lat, long recordings, even below the waterline with no GPS and when DVL obviously providing incremental positioning (inspite of EK3_SRC1_POSXY being set to GPS)… So a bit confused.

Other than the bung depth readings we seem to have the behaviour we want.

Results of a bit more testing today. With GPS fitted to the Navigator (Serial 3 with GPS_TYPE = Auto) and with the standard EK3 settings for DVL (see below), the GPS seems to set the current position automatically when GPS has lock. Hey presto! This kind of makes sense as the DVL is still doing it’s thing at the surface (if seafloor in range) so no real need to switch to GPS for controlling POSXY. We just need the GPS to provide the lat/long starting point for the DVL which it can merrily do until it fails, i.e goes underwater and looses a fix.

The only problem is depth reading gets messed up. Still trying to work out where the mysterious number comes from (note EK3_SRC1_POSZ set to Baro). Any ideas @EliotBR ?




Without a dedicated mode for it in ArduSub, I assume a Lua script would allow for the best available performance, because GPS units often give wild position estimates when they’re just below the water surface, which can throw things off before the switch to the DVL happens.

I asked @williangalvani, and he mentioned that VELZ is apparently leaking (:confused:), with the following PRs as potential fixes/improvements:

Thanks @EliotBR . Still a bit confused as to how or what is setting the lat/long from the GPS reading to the DVL A50 given GPS not included in the EK3_SRC1_* settings at all. Might that be the Waterlinked plugin code?

Whatever is setting the lat, long, I wonder if that could be beefed up to reject lat/long settings with low integrity (low nSat, poor HDOP)?

@williangalvani : Happy to test any potential fixes to the “VELZ leak” if useful and it speeds up a fix. Robust logging of a depth reading pretty critical for us.

1 Like

Another issue that is likely hurting us:

1 Like

Quick follow-up on this. @samuel_reedy dug into it a bit more and has posted this to Github: