diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 16771c7411..a9e3ae7c21 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1518,6 +1518,16 @@ void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &raw } } +// retrieve latest corrected optical flow samples (used for calibration) +bool NavEKF3::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const +{ + // return optical flow samples from primary core + if (core) { + return core[primary].getOptFlowSample(timeStamp_ms, flowRate, bodyRate, losPred); + } + return false; +} + // write yaw angle sensor measurements void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index bd17b0f1ce..68f12a04b8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -198,6 +198,9 @@ public: // posOffset is the XYZ flow sensor position in the body frame in m 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 frame linear and angular displacement measurements from a visual odometry sensor * diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 311a6aaebb..9fd2bcad59 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -737,6 +737,28 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus } } } + + // store optical flow rates for use in external calibration + flowCalSample.timestamp_ms = imuSampleTime_ms; + flowCalSample.flowRate.x = ofDataDelayed.flowRadXY.x; + flowCalSample.flowRate.y = ofDataDelayed.flowRadXY.y; + flowCalSample.bodyRate.x = ofDataDelayed.bodyRadXYZ.x; + flowCalSample.bodyRate.y = ofDataDelayed.bodyRadXYZ.y; + flowCalSample.losPred.x = losPred[0]; + flowCalSample.losPred.y = losPred[1]; +} + +// retrieve latest corrected optical flow samples (used for calibration) +bool NavEKF3_core::getOptFlowSample(uint32_t& timestamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const +{ + if (flowCalSample.timestamp_ms != 0) { + timestamp_ms = flowCalSample.timestamp_ms; + flowRate = flowCalSample.flowRate; + bodyRate = flowCalSample.bodyRate; + losPred = flowCalSample.losPred; + return true; + } + return false; } /******************************************************** diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index fe60f74110..26f1e13943 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -248,6 +248,9 @@ public: // posOffset is the XYZ flow sensor position in the body frame in m 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 frame linear and angular displacement measurements from a visual odometry sensor * @@ -1186,6 +1189,12 @@ private: ftype prevPosE; // east position at last measurement ftype varInnovRng; // range finder observation innovation variance (m^2) ftype innovRng; // range finder observation innovation (m) + struct { + uint32_t timestamp_ms; // system timestamp of last correct optical flow sample (used for calibration) + Vector2f flowRate; // latest corrected optical flow flow rate (used for calibration) + Vector2f bodyRate; // latest corrected optical flow body rate (used for calibration) + Vector2f losPred; // EKF estimated component of flowRate that comes from vehicle movement (not rotation) + } flowCalSample; ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m) bool inhibitGndState; // true when the terrain position state is to remain constant