diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 56c6b626d9..5915c3a6d2 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -2152,6 +2152,15 @@ void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &ra #endif } +// retrieve latest corrected optical flow samples (used for calibration) +bool AP_AHRS::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const +{ +#if HAL_NAVEKF3_AVAILABLE + return EKF3.getOptFlowSample(timeStamp_ms, flowRate, bodyRate, losPred); +#endif + return false; +} + // write body frame odometry measurements to the EKF void AP_AHRS::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset) { diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 2cfe163a22..03b4bb8dc3 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -240,6 +240,9 @@ public: // write optical flow measurements to EKF void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); + // retrieve latest corrected optical flow samples (used for calibration) + bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const; + // write body odometry measurements to the EKF void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);