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

AP_NavEKF : Add range measurement to EKF debug message

This commit is contained in:
priseborough 2014-10-16 05:56:48 +11:00 committed by Andrew Tridgell
parent f358d5e20f
commit 8dd1081f54
2 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3566,7 +3566,7 @@ bool NavEKF::getLLH(struct Location &loc) const
} }
// return data for debugging optical flow fusion // 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) const void NavEKF::getFlowDebug(float &scaleFactor, float &gndPos, float &flowX, float &flowY, float &omegaX, float &omegaY, uint8_t &quality, float &range) const
{ {
scaleFactor = flowStates[0]; scaleFactor = flowStates[0];
flowX = flowRadXY[0]; flowX = flowRadXY[0];
@ -3575,6 +3575,7 @@ void NavEKF::getFlowDebug(float &scaleFactor, float &gndPos, float &flowX, float
omegaY = omegaAcrossFlowTime.y;; omegaY = omegaAcrossFlowTime.y;;
gndPos = flowStates[1]; gndPos = flowStates[1];
quality = flowQuality; quality = flowQuality;
range = rngMea;
} }
// 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
@ -3963,7 +3964,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
flowMeaTime_ms = msecFlowMeas; flowMeaTime_ms = msecFlowMeas;
if (rawFlowQuality > 100){
flowQuality = rawFlowQuality; flowQuality = rawFlowQuality;
// recall vehicle states at mid sample time for flow observations allowing for delays // recall vehicle states at mid sample time for flow observations allowing for delays
RecallStates(statesAtFlowTime, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2); RecallStates(statesAtFlowTime, flowMeaTime_ms - _msecFLowDelay - flowTimeDeltaAvg_ms/2);
@ -3986,6 +3986,7 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// write flow rate measurements corrected for focal length scale factor errors and body rates // write flow rate measurements corrected for focal length scale factor errors and body rates
flowRadXYcomp[0] = flowStates[0]*flowRadXY[0] + omegaAcrossFlowTime.x; flowRadXYcomp[0] = flowStates[0]*flowRadXY[0] + omegaAcrossFlowTime.x;
flowRadXYcomp[1] = flowStates[0]*flowRadXY[1] + omegaAcrossFlowTime.y; flowRadXYcomp[1] = flowStates[0]*flowRadXY[1] + omegaAcrossFlowTime.y;
if (rawFlowQuality > 100){
// set flag that will trigger observations // set flag that will trigger observations
newDataFlow = true; newDataFlow = true;
} else { } else {

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -163,7 +163,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 &flowX, float &flowY, float &omegaX, float &omegaY, uint8_t &quality) const; void getFlowDebug(float &scaleFactor, float &gndPos, float &flowX, float &flowY, float &omegaX, float &omegaY, uint8_t &quality, float &range) const;
/* /*
return the filter fault status as a bitmasked integer return the filter fault status as a bitmasked integer