From c9e363d5298920b5bf35f423d389895edcb7ea35 Mon Sep 17 00:00:00 2001
From: chobits <chobits@itri.org.tw>
Date: Tue, 12 May 2020 14:06:24 +0800
Subject: [PATCH] AP_AHRS: support VISION_SPEED_ESTIMATE

---
 libraries/AP_AHRS/AP_AHRS.h          | 3 +++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 8 ++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   | 3 +++
 3 files changed, 14 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 3880e08e54..963e7407ea 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/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;
 
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index a1e109501f..86057d4214 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/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)
 {
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index b034d35b58..564eab71a6 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/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);