diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 01f05463ac..18e7a6c45f 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3612,14 +3612,14 @@ bool NavEKF::getHAGL(float &HAGL) const } // return data for debugging optical flow fusion -void NavEKF::getFlowDebug(float &dummy, float &estHAGL, float &flowInnovX, float &flowInnovY, float &flowVarX, float &flowVarY, float &rngInnov, float &range, float &gndOffsetErr) const +void NavEKF::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const { - dummy = 0.0f; - estHAGL = terrainState - state.position.z; + varFlow = max(flowTestRatio[0],flowTestRatio[1]); + gndOffset = terrainState; flowInnovX = innovOptFlow[0]; flowInnovY = innovOptFlow[1]; - auxInnovX = auxFlowObsInnov; - auxInnovY = auxFlowObsInnov; + auxInnov = auxFlowObsInnov; + HAGL = terrainState - state.position.z; rngInnov = innovRng; range = rngMea; gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset() diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 4901c0a2b3..c552a781ab 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -171,7 +171,7 @@ public: void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, uint8_t &rangeHealth, float &rawSonarRange); // return data for debugging optical flow fusion - void getFlowDebug(float &scaleFactor, float &gndPos, float &flowInnovX, float &flowInnovY, float &flowVarX, float &flowVarY, float &rngInnov, float &range, float &gndOffsetErr) const; + void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; /* return the filter fault status as a bitmasked integer