From 06c19a3a4d65d73839c53d456d49e6b33b5991eb Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 1 Nov 2014 13:27:22 +1100 Subject: [PATCH] AP_NavEKF : Update optical flow debug logging --- libraries/AP_NavEKF/AP_NavEKF.cpp | 12 ++++++------ libraries/AP_NavEKF/AP_NavEKF.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 71c2f5447e..1e6766fa7b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3622,15 +3622,15 @@ bool NavEKF::getLLH(struct Location &loc) const } // return data for debugging optical flow fusion -void NavEKF::getFlowDebug(float &scaleFactor, float &gndPos, float &flowX, float &flowY, float &omegaX, float &omegaY, uint8_t &quality, float &range) const +void NavEKF::getFlowDebug(float &scaleFactor, float &gndPos, float &flowInnovX, float &flowInnovY, float &augFlowInnovX, float &augFlowInnovY, float &rngInnov, float &range) const { scaleFactor = flowStates[0]; - flowX = flowRadXY[0]; - flowY = flowRadXY[1]; - omegaX = omegaAcrossFlowTime.x;; - omegaY = omegaAcrossFlowTime.y;; gndPos = flowStates[1]; - quality = flowQuality; + flowInnovX = innovOptFlow[0]; + flowInnovY = innovOptFlow[1]; + augFlowInnovX = auxFlowObsInnov[0]; + augFlowInnovY = auxFlowObsInnov[1]; + rngInnov = innovRng; range = rngMea; } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 770d52b8ef..1cedaa026b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -163,7 +163,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 &flowX, float &flowY, float &omegaX, float &omegaY, uint8_t &quality, float &range) const; + void getFlowDebug(float &scaleFactor, float &gndPos, float &flowInnovX, float &flowInnovY, float &augFlowInnovX, float &augFlowInnovY, float &rngInnov, float &range) const; /* return the filter fault status as a bitmasked integer