From 224792b8b88eb0b3d75df90eb589b0eb91da3cc7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Nov 2020 09:30:31 +1100 Subject: [PATCH] AP_NavEKF2: added optflow support for AP_DAL --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 0eb32e7e58..27104ebb1b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -121,6 +121,8 @@ void NavEKF2_core::readRangeFinder(void) // this needs to be called externally. void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset) { + AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset); + // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update // The PX4Flow sensor outputs flow rates with the following axis and sign conventions: // A positive X rate is produced by a positive sensor rotation about the X axis