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

AP_NavEKF: Update EKF optical flow data logging

This commit is contained in:
priseborough 2015-01-03 13:20:09 +11:00 committed by Andrew Tridgell
parent b651eac48d
commit 69e86d3ea4
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3668,16 +3668,17 @@ bool NavEKF::getHAGL(float &HAGL) const
} }
// return data for debugging optical flow fusion // return data for debugging optical flow fusion
void NavEKF::getFlowDebug(float &scaleFactor, float &gndPos, float &flowInnovX, float &flowInnovY, float &augFlowInnovX, float &augFlowInnovY, float &rngInnov, float &range) const void NavEKF::getFlowDebug(float &scaleFactor, float &estHAGL, float &flowInnovX, float &flowInnovY, float &flowVarX, float &flowVarY, float &rngInnov, float &range, float &gndOffsetErr) const
{ {
scaleFactor = flowStates[0]; scaleFactor = flowStates[0];
gndPos = flowStates[1]; estHAGL = flowStates[1] - state.position.z;
flowInnovX = innovOptFlow[0]; flowInnovX = innovOptFlow[0];
flowInnovY = innovOptFlow[1]; flowInnovY = innovOptFlow[1];
augFlowInnovX = auxFlowObsInnov[0]; flowVarX = flowTestRatio[0];
augFlowInnovY = auxFlowObsInnov[1]; flowVarY = flowTestRatio[1];
rngInnov = innovRng; rngInnov = innovRng;
range = rngMea; range = rngMea;
gndOffsetErr = sqrtf(Popt[1][1]); // note Popt[1][1] is constrained to be non-negative in RunAuxiliaryEKF()
} }
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed // calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed

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 &augFlowInnovX, float &augFlowInnovY, float &rngInnov, float &range) const; void getFlowDebug(float &scaleFactor, float &gndPos, float &flowInnovX, float &flowInnovY, float &flowVarX, float &flowVarY, 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