Allow using SPEED_ESTIMATE messages

[ES-Alexander]

SPEED_ESTIMATE was previously disallowed because MAVLink2REST didn't support NaN values, but that has been resolved, and should be fixed in BlueOS since 1.1.0, which is far enough behind latest stable (1.2.5) that it seems reasonable to assume people have updated, especially if they're wanting to update to a new Extension version.

This PR:
1 Un-hides the frontend message type switching

1. Implements "unknown" covariance defaults for the relevant MAVLink messages
2. Supports retrieving velocity covariance values from a WL-DVL that provides the field
   • The value order should be confirmed (MAVLink specifies row-major order, where the first three values are [vx, vy, vz; ...], but the WL protocol docs do not specify
     • EDIT: WL have fairly pointed out that covariance matrices are symmetric, so this shouldn't be relevant (although I am still a bit miffed as to why MAVLink specifies an order and all 9 terms, when the position covariance matrices are specified with only the upper right triangle...)
3. Fixes a code case the IDE was complaining was unreachable because

   This condition will always return 'false' since JavaScript compares objects by reference, not value.

   • ⚠️ I'm insufficiently confident with my javascript and the context of the relevant code to know whether that warning was correct - the commit can be removed if the code was ok as-is
     Corrects the rangefinder type, as ultrasonic instead of laser-based

• Enabled support for operating the DVL in forward orientation
  • ⚠️ possibly worth checking the velocity covariance matrix ordering/negations
  • ⚠️ ⚠️ did not add handling support to handle_position_local - I'm unsure what needs doing there, because I don't know whether the DVL's internal EKF accounts for the sensor's orientation in its position estimates, which is what I've currently assumed (quite possibly incorrectly)

[ES-Alexander]

@Williangalvani did some testing, and the SPEED_ESTIMATE option was sending the data through but wasn't allowing position functionality, and the POSITION_ESTIMATE option was having issues with MAVLink2REST.

We'll need to do some more investigating before this can be merged.

• It's possible position-enabled modes require position/position_delta estimates,
  • If that's not the case then there may be some issue with how the data is being put into the MAVLink message
  • If it is the case then it might be worth allowing the user to select speed as an independent option from the position/position_delta "one only" group
• Python's default NaN representation may not match what MAVLink2REST is set up to make sense of, in which case the POSITION_ESTIMATE issue could be with the provided unknown covariance, which uses NaN in the first value because the DVL does not provide covariances for the dead reckoning
  • A reasonable test would probably be to send a message through CURL, and seeing whether MAVLink2REST accepts it when the NaN is removed, or if it's printed in a different format (e.g. NaN instead of nan, and/or with string quotations)

[Williangalvani]

another note: it looks like the covariance isn't consumed for speed_estimate:
https://github.com/ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/GCS_Common.cpp#L3857-L3865

[Williangalvani]

I just realized. this also made the UI weird:

[benjiboy50fonz]

Any updates on this issue? I'm experiencing similar problems.

[wlseb]

After Willian's comment on Jul 12th that the Covariance in VISION_SPEED_ESTIMATE is actually not used in the ardupliot code I was curious how it actually works. I had a closer look where the different messages end up. What I understand now (please correct if wrong):

Current datapath:

• mavlink VISION_POSITION_DELTA uses MAV_FRAME_BODY_FRD (body forward-right-down)
• The DVL outputs velocities in a body-frame so it makes sense to forward those as VISION_POSITION_DELTA
• VISION_POSITION_DELTA ends up as BodyFrameOdometry in the Ardupilot EKF3-core and is converted back to a velocity there.
• bodyOdmDataNew.angRate (ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp) is the last place in the dataflow the angular rate is forwarded but it is not used when fusing the body velocity in FuseBodyVel(). This means no DVL-IMU-data is currently used in the VISION_POSITION_DELTA-mode (this also means DVL-gyro calibration is not important on a BlueROV).
• The only downside of using this datapath is that not the full covariance matrix provided by the DVL is forwarded to the EKF, only a scalar value.

Alternatives:

• mavlink VISION_SPEED_ESTIMATE and mavlink VISION_POSITION_ESTIMATE are supposed to provide data in a global/local coordinate system respectively
• The DVL outputs also a local position after dead-reckoning with its IMU. The best matching message would be VISION_POSITION_ESTIMATE. It is however preferable to use the main vehicle IMU so the user does not have to care about another gyro calibration.
• To send VISION_SPEED_ESTIMATE one would need to perform a coordinate transformation in the extension. I think that is not a good idea due to latencies. As Willian wrote, the covariance matrix sent through Mavlink is not used by Ardupilot either right now.

So my conclusion is basically that the DVL extension is as good as it can be now without changing Ardupilot code in addition. 🙂