zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: Update flow debug logging

mission-4.1.18
priseborough 10 years ago committed by Andrew Tridgell
parent
8d1dae3ac1
commit
9f4baaa865
2 changed files with 6 additions and 6 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 2
      libraries/AP_NavEKF/AP_NavEKF.h

10
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape 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
Unescape 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

Write Preview
Loading…
Cancel
Save