diff --git a/dev/source/docs/mavlink-nongps-position-estimation.rst b/dev/source/docs/mavlink-nongps-position-estimation.rst index 5f790d732d..0c3d9ab9fa 100644 --- a/dev/source/docs/mavlink-nongps-position-estimation.rst +++ b/dev/source/docs/mavlink-nongps-position-estimation.rst @@ -12,7 +12,7 @@ This is also called "External Navigation" although to be more precise it involve The user wiki pages for :ref:`Non-GPS navigation is here ` and :ref:`GPS/Non-GPS transitions is here ` -Any of the following messages should be sent in at 4hz or higher: +Any of the following messages should be sent to the autopilot at 4hz or higher: - `ODOMETRY `__ (the preferred method) - `VISION_POSITION_ESTIMATE `__ and optionally `VISION_SPEED_ESTIMATE `__ @@ -116,10 +116,10 @@ The preferred method is to send an `ODOMETRY Yaw angular speed in rad/s (clockwise is positive) - + pos_covariance float[21] - not used + elements 0, 6 and 11 are x, y and z-axis position error. 15, 18, 20 are roll, pitch and yaw angle error. Ignored if NaN @@ -145,3 +145,92 @@ The preferred method is to send an `ODOMETRY + +Six "pos_covariance" array elements are used to calculate the position error and angle error + +- 0: x-axis position error (m) +- 6: y-axis position error (m) +- 11: z-axis position error (m) +- 15: roll angle error (rad) +- 18: pitch angle error (rad) +- 20: yaw angle error (rad) + +For both position and angle error, the 3 elements are combined to create a single error which is used during EKF sensor fusion + +.. image:: ../images/nongps-position-estimation-poserr.png + :target: ../_images/nongps-position-estimation-poserr.png + +In addition the position error is constrained to be between :ref:`VISO_POS_M_NSE ` and 100 and the angle error between :ref:`VISO_YAW_M_NSE ` and 1.5 + +Messages with "quality" below the :ref:`VISO_QUAL_MIN ` parameter value are ignored + +OPTICAL_FLOW message +-------------------- + +An offboard optical flow sensor can send its horizontal flow sensor data to the autopilot using the `OPTICAL_FLOW `__ message + +Users should set :ref:`FLOW_TYPE` = 5 (MAVLink) and then perform the :ref:`Optical Flow Setup procedure ` before use + +.. raw:: html + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Field NameTypeDescription
time_usecuint64_tTimestamp since system boot. This does not need to be syncronised with the autopilot's time
sensor_iduint8_tnot used
flow_xint16_tFlow rate around X-axis
flow_xint16_tFlow rate around Y-axis
flow_comp_m_xfloatnot used
flow_comp_m_yfloatnot used
qualityuint8_tOptical flow quality 0:bad, 255:best
ground_distancefloatnot used
flow_rate_xfloatnot used
flow_rate_yfloatnot used
+ +Please see this `issue 29131 `__ for improvements we should make to the handling of this message diff --git a/dev/source/images/nongps-position-estimation-poserr.png b/dev/source/images/nongps-position-estimation-poserr.png new file mode 100644 index 0000000000..12a36234e1 Binary files /dev/null and b/dev/source/images/nongps-position-estimation-poserr.png differ