From c0df9990947da89df2e60b522a41efc4ce5772be Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Mon, 24 Oct 2022 14:01:04 +1100 Subject: [PATCH] AP_NavEKF2: Add support for fixed height optical flow --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 7 ++++--- libraries/AP_NavEKF2/AP_NavEKF2.h | 3 ++- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 4 +++- libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 8 ++++++++ libraries/AP_NavEKF2/AP_NavEKF2_core.h | 4 +++- 5 files changed, 20 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index b9a3b17126..29e308bc4a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1199,13 +1199,14 @@ bool NavEKF2::use_compass(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 NavEKF2::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 NavEKF2::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; i +#include #include "AP_NavEKF2.h" #include "AP_NavEKF2_core.h" @@ -315,6 +316,13 @@ void NavEKF2_core::FuseOptFlow() range -= posOffsetEarth.z / prevTnb.c.z; } + // override with user specified height (if given, for rover) +#if APM_BUILD_TYPE(APM_BUILD_Rover) + if (ofDataDelayed.heightOverride > 0) { + range = ofDataDelayed.heightOverride; + } +#endif + // calculate relative velocity in sensor frame including the relative motion due to rotation relVelSensor = prevTnb*stateStruct.velocity + ofDataDelayed.bodyRadXYZ % posOffsetBody; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index e25bad340a..8c74d18fb3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -199,7 +199,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); /* Returns the following data for debugging range beacon fusion @@ -461,6 +462,7 @@ private: Vector2F flowRadXYcomp; Vector3F bodyRadXYZ; Vector3F body_offset; + float heightOverride; }; struct ext_nav_elements : EKF_obs_element_t {