bluerobotics / blueos-water-linked-dvl Goto Github PK

GPS origin can only be specified once, but it should still be possible to specify the vehicle position relative to that position, including multiple times.

Additional discussion at ArduPilot/ardupilot#20991

For explanation purposes, we need to make clear that:

1. GPS (EKF) origin is the prime accuracy point (e.g. best to set in the centre of the expected travel area)
2. Vehicle position is the position at any given moment, and can be 'corrected' / set whenever a reasonable value is available

This may be reasonably simple to add using the existing position estimate message that's set up to handle dead reckoning messages.

Original Companion UGPS support contains several overlaps with the DVL-A50 driver here. Since both devices are from Water Linked, it likely makes sense to have both devices available via the same package, which can asynchronously monitor for both. That may also lend itself to easier integration between the DVL and UGPS in future.

Line 6 is # Install gstreamer, but should be something like # Install dvl service.

DISTANCE_SENSOR.signal_quality is always 0, so consumers don't have any information about confidence, etc.

I see code to calculate 1-100 confidence from the figure-of-merit, perhaps we can interpret this as signal_quality? I haven't logged this data to see if it's a reasonable idea, but I hope to do so soon.

Unable to pan the map without the drop pin shifting.

Sometimes ardupilot doesn't accept the new position. The message might be lost.

The Latitude and Longitude text boxes allow manually specifying an origin / position, but doing so does not update the pin position in the UI, which is unintuitive and may cause some usage issues.

Raised in this forum post.

As something of a side note, it could be useful to have an indication of whether the origin or position is currently being set (which would make the actual behaviour clearer). Would presumably require checking if origin is set before creating the interface, then setting the map title and the submit button text to use 'EKF origin' or 'position' as relevant.

• brought up in #5
• basic internal support added in #6
• needs rework for full internal support, and frontend to give access to user

If QGC is not running, then pressing the SET VEHICLE POSITION button twice will fail. This is because DvlDriver.set_current_position() is trying to get /VFR_HUD/message/alt, but the autopilot is not sending VFR_HUD messages.

I tested this on a Pi3 + Pixhawk bench setup with no external sensors. I recently re-installed BlueOS.

To repro:

1. Install the WL DVL extension
2. Select SET VEHICLE POSITION twice. The first time DvlDriver.has_origin_set() will return false and a SET_GPS_GLOBAL_ORIGIN message will be sent. The 2nd time DvlDriver.has_origin_set() will return true, but no GLOBAL_VISION_POSITION_ESTIMATE message will be sent.

From the docker container logs:

2023-09-04 20:28:59.239 | WARNING  | dvl:has_origin_set:218 - Unable to read current time for GPS_GLOBAL_ORIGIN, using 0: could not convert string to float: 'None'
2023-09-04 20:28:59.241 | DEBUG    | dvl:has_origin_set:222 - Trying to read origin, try # 0
2023-09-04 20:28:59.800 | INFO     | dvl:set_current_position:245 - Origin has already been set, sending POSITION_ESTIMATE instead
[2023-09-04 20:28:59,808] ERROR in app: Exception on /setcurrentposition/47.60764953320023/-122.34408152105063 [GET]
Traceback (most recent call last):
  File "/usr/local/lib/python3.9/site-packages/flask/app.py", line 2311, in wsgi_app
    response = self.full_dispatch_request()
  File "/usr/local/lib/python3.9/site-packages/flask/app.py", line 1834, in full_dispatch_request
    rv = self.handle_user_exception(e)
  File "/usr/local/lib/python3.9/site-packages/flask/app.py", line 1737, in handle_user_exception
    reraise(exc_type, exc_value, tb)
  File "/usr/local/lib/python3.9/site-packages/flask/_compat.py", line 36, in reraise
    raise value
  File "/usr/local/lib/python3.9/site-packages/flask/app.py", line 1832, in full_dispatch_request
    rv = self.dispatch_request()
  File "/usr/local/lib/python3.9/site-packages/flask/app.py", line 1818, in dispatch_request
    return self.view_functions[rule.endpoint](**req.view_args)
  File "/home/pi/dvl-a50/main.py", line 96, in set_current_position
    return str(api.set_current_position(lat, lon))
  File "/home/pi/dvl-a50/main.py", line 56, in set_current_position
    return self.dvl.set_current_position(float(lat), float(lon))
  File "/home/pi/dvl-a50/dvl.py", line 248, in set_current_position
    depth = float(self.mav.get("/VFR_HUD/message/alt"))
ValueError: could not convert string to float: 'None'
192.168.2.1 - - [04/Sep/2023 20:28:59] "GET /setcurrentposition/47.60764953320023/-122.34408152105063 HTTP/1.1" 500 -

I think the fix is to add something like this to DvlDriver.setup_mavlink():

        self.mav.ensure_message_frequency("VFR_HUD", 74, 5)

DISTANCE_SENSOR and VISION_POSITION_DELTA msgs are sent from sysid=255, compid=0. I believe that this should be sysid=1 (the autopilot system), and a reasonable component id.

The current settings work, but they make log analysis a bit cumbersome.

MAVLink ArduPilotMega VISION_POSITION_DELTA.angle_delta field should be radians, but mavllink2resthelper.send_vision sends degrees.

mavllink2resthelper.send_vision_position_estimate does the conversion correctly.

I found this when investigating if / how we can fuse the DVL compass readings with the Navigator compass readings.

I've been working on fusing data from the WL UGPS and WL DVL and I'm getting good results in simulation. However, I see that the DVL extension is setting EK3_SRC1_POSXY and EK3_SRC1_VELXY to 6 (ExternalNav) every time it starts up, which complicates field testing.

In my simulations, these settings produce nice fusion:

1	1	EK3_SRC1_POSXY	3	2	# GPS
1	1	EK3_SRC1_POSZ	1	2	# Baro
1	1	EK3_SRC1_VELXY	3	2	# GPS
1	1	EK3_SRC1_VELZ	0	2	# None
1	1	EK3_SRC1_YAW	1	2	# Compass
1	1	EK3_SRC2_POSXY	0	2	# None
1	1	EK3_SRC2_POSZ	0	2	# None
1	1	EK3_SRC2_VELXY	6	2	# ExternalNav
1	1	EK3_SRC2_VELZ	0	2	# None
1	1	EK3_SRC2_YAW	0	2	# None
1	1	EK3_SRC3_POSXY	0	2	# None
1	1	EK3_SRC3_POSZ	0	2	# None
1	1	EK3_SRC3_VELXY	0	2	# None
1	1	EK3_SRC3_VELZ	0	2	# None
1	1	EK3_SRC3_YAW	0	2	# None
1	1	EK3_SRC_OPTIONS	1	4	# FuseAllVelocities

These settings use the UGPS for POSXY, and then use both UGPS and DVL for VELXY, as the FuseAllVelocities bit is set.

The workaround is to start the extension, then go into QGC and undo the changes.

Another workaround is to flip SRC1 and SRC2, and use a Python script (or some other method) to send a MAV_CMD_SET_EKF_SOURCE_SET message to ArduSub after it boots up to make SRC2 the primary source set:

1	1	EK3_SRC1_POSXY	6	2	# ExternalNav
1	1	EK3_SRC1_POSZ	1	2	# Baro
1	1	EK3_SRC1_VELXY	6	2	# ExternalNav
1	1	EK3_SRC1_VELZ	0	2	# None
1	1	EK3_SRC1_YAW	1	2	# Compass
1	1	EK3_SRC2_POSXY	3	2	# GPS
1	1	EK3_SRC2_POSZ	1	2	# Baro
1	1	EK3_SRC2_VELXY	3	2	# GPS
1	1	EK3_SRC2_VELZ	0	2	# None
1	1	EK3_SRC2_YAW	1	2	# Compass
1	1	EK3_SRC3_POSXY	0	2	# None
1	1	EK3_SRC3_POSZ	0	2	# None
1	1	EK3_SRC3_VELXY	0	2	# None
1	1	EK3_SRC3_VELZ	0	2	# None
1	1	EK3_SRC3_YAW	0	2	# None
1	1	EK3_SRC_OPTIONS	1	4	# FuseAllVelocities

Both of these workarounds are complicated.

The current behavior provides a nice out-of-the-box experience for first-time DVL users, but perhaps there's some way to make this optional?

Discovered two bugs in the mavlink2resthelper.py file:

1. Import in line 1 references companionhelper, should be blueoshelper.

2. The GET requests in line 175 and 186 are failing due to incomplete paths, likely due to BlueOS using a newer mavlink2rest version. Can be fixed by adding the constant:
   PATH_PREFIX = "/vehicles/1/components/1/messages"

And updating the aforementioned lines to:
response = request(MAVLINK2REST_URL + '/mavlink' + PATH_PREFIX + path)

Ideally the path prefix should allow for accessing different vehicle and components IDs, unsure how to best implement this.

This forum post found that setting the GPS origin in the Water Linked DVL interface with a -220 longitude didn't work.

@Williangalvani suspects that it may require a value between -180 to 180. If confirmed, this package should wrap the value before displaying to the user and/or before sending to the autopilot to ensure correct functionality.

Currently some valid protocol features are not supported or tested, including

• dead reckoning
  • reset dead reckoning command available via the DVL GUI in the frontend :-)
  • ArduPilot's VISION_POSITION_ESTIMATE and GLOBAL_POSITION_ESTIMATE seem to be handled equally, so local positions may not be supported - needs confirmation/work, and can then be exposed as an output option via the frontend
• gyro calibration command available via the DVL GUI in the frontend :-)
• handling different protocol versions

The best integrations are feature-complete, which this one is not.

Seems like the asset isn't loading properly or something

Tested on Google Chrome and Orion browsers - same issue on both.
Intel-based MacBook running macOS 12.6.

The lat/lon values doesn't get populated when the pin is dragged and dropped for the first time. It works as intended only after the second time of moving the pin on map.

Presently the angles sent to ArduSub are hard-coded as 0s, which may be messing up its attitude estimation and causing false spinning measurements. From internal discussions, the zero-values were initially put in place at a time when the DVL-A50 did not have externally available attitude estimates, and feeding back ArduSub's own estimates exacerbated gyro drift, so zero was used as a temporarily helpful compromise.

Ideally velocity messages from the DVL should probably be sent onwards as VISION_SPEED_ESTIMATE messages to ArduSub, but if VISION_POSITION_DELTA messages are selected / must be used then perhaps the angles from the dead reckoning reports can be used (may require calibrate gyro to be implemented, and possibly reset dead reckoning too, for robustness).

This forum post may be an example of this issue occurring.