Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Update flow debug logging

This commit is contained in:
priseborough 2015-01-04 10:27:07 +11:00 committed by Andrew Tridgell
parent 8d1dae3ac1
commit 9f4baaa865
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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()

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -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