diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index ea7e84a882..08adf4ad67 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -317,5 +317,10 @@ bool AP_AHRS_NavEKF::initialised(void) const return (ekf_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); }; +// write optical flow data to EKF +void AP_AHRS_NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, float &rawSonarRange, uint32_t &msecFlowMeas) +{ + EKF.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawSonarRange, msecFlowMeas); +} #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 69083caa38..f5a113730a 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -101,6 +101,9 @@ public: bool get_velocity_NED(Vector3f &vec) const; bool get_relative_position_NED(Vector3f &vec) const; + // write optical flow measurements to EKF + void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, float &rawSonarRange, uint32_t &msecFlowMeas); + void set_ekf_use(bool setting) { _ekf_use.set(setting); } // is the AHRS subsystem healthy?