From abcacec25fc8a8d1558eae2d99cfa58cb4b9f74a Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Mon, 24 Oct 2022 14:01:08 +1100 Subject: [PATCH] AP_NavEKF3: Add support for fixed height optical flow --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 7 ++++--- libraries/AP_NavEKF3/AP_NavEKF3.h | 3 ++- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 +++- libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 7 +++++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 +++- 5 files changed, 19 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 7219ba6c62..958b59b3a3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1526,13 +1526,14 @@ bool NavEKF3::configuredToUseGPSForPosXY(void) const // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. // posOffset is the XYZ flow sensor position in the body frame in m -void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset) +// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used +void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride) { - AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset); + AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride); if (core) { for (uint8_t i=0; isensorIntervalMin_ms) { @@ -216,6 +216,8 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) // write the flow sensor position in body frame ofDataNew.body_offset = posOffset.toftype(); + // write the flow sensor height override + ofDataNew.heightOverride = heightOverride; // write flow rate measurements corrected for body rates ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x; ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 28545ef5aa..e457bf7a03 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -303,6 +303,13 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody); range -= posOffsetEarth.z / prevTnb.c.z; } + +#if APM_BUILD_TYPE(APM_BUILD_Rover) + // override with user specified height (if given, for rover) + if (ofDataDelayed.heightOverride > 0) { + range = ofDataDelayed.heightOverride; + } +#endif // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 8460818830..e84ea4e3db 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -252,7 +252,8 @@ public: // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. // 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); + // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used + void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride); // retrieve latest corrected optical flow samples (used for calibration) bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const; @@ -570,6 +571,7 @@ private: Vector2F flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec) Vector3F bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec) Vector3F body_offset; // XYZ position of the optical flow sensor in body frame (m) + float heightOverride; // The fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used }; struct vel_odm_elements : EKF_obs_element_t {