AP_AHRS: support VISION_SPEED_ESTIMATE

libraries/AP_AHRS/AP_AHRS.h

@@ -559,6 +559,9 @@ public:
     // Write position and quaternion data from an external navigation system
     virtual void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) { }
 
+    // Write velocity data from an external navigation system
+    virtual void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { }
+
     // return current vibration vector for primary IMU
     Vector3f get_vibration(void) const;
 

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -1474,6 +1474,14 @@ void AP_AHRS_NavEKF::writeDefaultAirSpeed(float airspeed)
 #endif
 }
 
+// Write velocity data from an external navigation system
+void AP_AHRS_NavEKF::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
+{
+#if HAL_NAVEKF2_AVAILABLE
+    EKF2.writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
+#endif
+}
+
 // inhibit GPS usage
 uint8_t AP_AHRS_NavEKF::setInhibitGPS(void)
 {

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -186,6 +186,9 @@ public:
     // Write position and quaternion data from an external navigation system
     void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) override;
 
+    // Write velocity data from an external navigation system
+    void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) override;
+
     // inhibit GPS usage
     uint8_t setInhibitGPS(void);