Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Update flow debug logging

Browse Source
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 // 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; varFlow = max(flowTestRatio[0],flowTestRatio[1]);
estHAGL = terrainState - state.position.z; gndOffset = terrainState;
flowInnovX = innovOptFlow[0]; flowInnovX = innovOptFlow[0];
flowInnovY = innovOptFlow[1]; flowInnovY = innovOptFlow[1];
auxInnovX = auxFlowObsInnov; auxInnov = auxFlowObsInnov;
auxInnovY = auxFlowObsInnov; HAGL = terrainState - state.position.z;
rngInnov = innovRng; rngInnov = innovRng;
range = rngMea; range = rngMea;
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset() 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); void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, uint8_t &rangeHealth, float &rawSonarRange);
// return data for debugging optical flow fusion // 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 return the filter fault status as a bitmasked integer