Add velocity observations to external vision interface

EKF/common.h

@@ -80,10 +80,12 @@ struct flow_message {
 };
 
 struct ext_vision_message {
-	Vector3f posNED;	///< measured NED position relative to the local origin (m)
-	Quatf quat;		///< measured quaternion orientation defining rotation from NED to body frame
+	Vector3f posNED;	///< XYZ position in earth frame (m) - Z must be aligned with down axis
+	Vector3f velNED;	///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
+	Quatf quat;		///< quaternion defining rotation from body to earth frame
 	float posErr;		///< 1-Sigma horizontal position accuracy (m)
 	float hgtErr;		///< 1-Sigma height accuracy (m)
+	float velErr;		///< 1-Sigma velocity accuracy (m/sec)
 	float angErr;		///< 1-Sigma angular error (rad)
 };
 
@@ -151,10 +153,12 @@ struct flowSample {
 };
 
 struct extVisionSample {
-	Vector3f posNED;	///< measured NED position relative to the local origin (m)
-	Quatf quat;		///< measured quaternion orientation defining rotation from NED to body frame
+	Vector3f posNED;	///< XYZ position in earth frame (m) - Z must be aligned with down axis
+	Vector3f velNED;	///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
+	Quatf quat;		///< quaternion defining rotation from body to earth frame
 	float posErr;		///< 1-Sigma horizontal position accuracy (m)
 	float hgtErr;		///< 1-Sigma height accuracy (m)
+	float velErr;		///< 1-Sigma velocity accuracy (m/sec)
 	float angErr;		///< 1-Sigma angular error (rad)
 	uint64_t time_us;	///< timestamp of the measurement (uSec)
 };

EKF/estimator_interface.cpp

@@ -442,9 +442,11 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
 		// copy required data
 		ev_sample_new.angErr = evdata->angErr;
 		ev_sample_new.posErr = evdata->posErr;
+		ev_sample_new.velErr = evdata->velErr;
 		ev_sample_new.hgtErr = evdata->hgtErr;
 		ev_sample_new.quat = evdata->quat;
 		ev_sample_new.posNED = evdata->posNED;
+		ev_sample_new.velNED = evdata->velNED;
 
 		// record time for comparison next measurement
 		_time_last_ext_vision = time_usec;