diff --git a/dev/source/docs/mavlink-commands.rst b/dev/source/docs/mavlink-commands.rst index e9a9d2f4ee..5d2672b6b7 100644 --- a/dev/source/docs/mavlink-commands.rst +++ b/dev/source/docs/mavlink-commands.rst @@ -22,6 +22,7 @@ ArduPilot supports the MAVLink protocol for communication with Ground Stations a Gimbal / Camera Mount Mission Upload/Download Move a Servo + Non-GPS Position Estimation Winch Commands MAVLink Routing Other Commands diff --git a/dev/source/docs/mavlink-nongps-position-estimation.rst b/dev/source/docs/mavlink-nongps-position-estimation.rst new file mode 100644 index 0000000000..5f790d732d --- /dev/null +++ b/dev/source/docs/mavlink-nongps-position-estimation.rst @@ -0,0 +1,147 @@ +.. _mavlink-nongps-position-estimation: + +=========================== +Non-GPS Position Estimation +=========================== + +This page explains how MAVLink can be used to send in external position and velocity estimates to ArduPilot's EKF allowing it to maintain a position estimate and thus control itself without a GPS + +This is also called "External Navigation" although to be more precise it involves estimation rather than navigation or control. + +.. note:: + + 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: + +- `ODOMETRY `__ (the preferred method) +- `VISION_POSITION_ESTIMATE `__ and optionally `VISION_SPEED_ESTIMATE `__ +- `VISION_POSITION_DELTA `__ +- `VICON_POSITION_ESTIMATE `__ +- `ATT_POS_MOCAP `__ +- `GLOBAL_VISION_POSITION_ESTIMATE `__ (not recommended) +- `GPS_INPUT `__ (not recommended) + +ArduPilot's parameters should be setup as if a :ref:`ModalAI VOXL ` is used which includes: + +- Set :ref:`VISO_TYPE ` = 3 (VOXL) +- Set :ref:`VISO_POS_X `, :ref:`VISO_POS_Y `, :ref:`VISO_POS_Z ` to the camera's position on the vehicle +- Set :ref:`EK3_SRC1_POSXY ` = 6 (ExternalNav) +- Set :ref:`EK3_SRC1_VELXY ` = 6 (ExternalNav) or 0 (None) +- Set :ref:`EK3_SRC1_POSZ ` = 6 (ExternalNav) or 1 (Baro) +- Set :ref:`EK3_SRC1_VELZ ` = 6 (ExternalNav) or 0 (None) +- Set :ref:`EK3_SRC1_YAW ` = 6 (ExternalNav) or 1 (Compass) + +In addition it may be useful to setup :ref:`GPS/Non-GPS transitions ` to allow switching between GPS and External Navigation + +If no GPS is attached to the autopilot then the :ref:`EKF origin must be set ` before the EKF can start estimating its position. If the vehicle will always be flown at the same location the `ahrs-set-origin.lua script `__ may be used + +ODOMETRY message +---------------- + +The preferred method is to send an `ODOMETRY `__ with the fields populated as described below: + +.. raw:: html + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Field NameTypeDescription
time_usecuint64_tTimestamp since system boot. This does not need to be syncronised with the autopilot's time
frame_iduint8_tMAV_FRAME_BODY_FRD (12) or MAV_FRAME_LOCAL_FRD (20)
child_frame_iduint8_tMAV_FRAME_BODY_FRD (12) or MAV_FRAME_LOCAL_FRD (20)
xfloatX position in meters
yfloatY position in meters
zfloatZ position in meters (positive is down)
qfloat[4]Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)
vxfloatX axis linear speed in m/s
vyfloatY axis linear speed in m/s
vzfloatZ axis linear speed in m/s (positive is down)
rollspeedfloatRoll angular speed in rad/s (backwards is positive)
pitchspeedfloatPitch angular speed in rad/s (forward is positive)
yawspeedfloatYaw angular speed in rad/s (clockwise is positive)
pos_covariancefloat[21]not used
velocity_covariancefloat[21]not used
reset_counteruint8_tExternal estimator reset counter. This should be incremented when the estimate resets position, velocity, attitude or angular speed
estimator_typeuint8_tnot used
qualityuint8_tquality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality