aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Apply timeout to terrain offset validity reporting

Browse Source 
The terrain offset solution status is usable for a short period of time without state updates so a timeout has been added which prevents the rapid changes in solution status due to short duration sensor read errors.
This commit is contained in:
priseborough 2015-01-03 09:12:23 +11:00 committed by Andrew Tridgell
parent e6474d676e
commit b651eac48d
2 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2542,10 +2542,12 @@ void NavEKF::RunAuxiliaryEKF()
// calculate a predicted LOS rate squared // calculate a predicted LOS rate squared
float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]); float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]);
// don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable // don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable
// record the time we last updated the terrain offset state
if ((losRateSq < 0.01f || PV_AidingMode == AID_RELATIVE) && !fuseRngData) { if ((losRateSq < 0.01f || PV_AidingMode == AID_RELATIVE) && !fuseRngData) {
inhibitGndState = true; inhibitGndState = true;
} else { } else {
inhibitGndState = false; inhibitGndState = false;
gndHgtValidTime_ms = imuSampleTime_ms;
} }
// Don't update focal length offset if there is no range finder or not using GPS, or we are not flying fast enough to generate a useful LOS rate // Don't update focal length offset if there is no range finder or not using GPS, or we are not flying fast enough to generate a useful LOS rate
if (!fuseRngData || PV_AidingMode == AID_RELATIVE || losRateSq < 0.01f || (flowStates[1] - state.position[2]) < 3.0f) { if (!fuseRngData || PV_AidingMode == AID_RELATIVE || losRateSq < 0.01f || (flowStates[1] - state.position[2]) < 3.0f) {
@ -4092,7 +4094,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
statesAtRngTime = statesAtFlowTime; statesAtRngTime = statesAtFlowTime;
rngMea = rawSonarRange; rngMea = rawSonarRange;
newDataRng = true; newDataRng = true;
rngMeaTime_ms = imuSampleTime_ms;
} else { } else {
newDataRng = false; newDataRng = false;
} }
@ -4304,7 +4305,7 @@ void NavEKF::InitialiseVariables()
flowValidMeaTime_ms = imuSampleTime_ms; flowValidMeaTime_ms = imuSampleTime_ms;
flowMeaTime_ms = 0; flowMeaTime_ms = 0;
prevFlowFusionTime_ms = imuSampleTime_ms; prevFlowFusionTime_ms = imuSampleTime_ms;
rngMeaTime_ms = imuSampleTime_ms; gndHgtValidTime_ms = 0;
ekfStartTime_ms = imuSampleTime_ms; ekfStartTime_ms = imuSampleTime_ms;
// initialise other variables // initialise other variables
@ -4518,13 +4519,14 @@ void NavEKF::getFilterStatus(uint8_t &status) const
bool notDeadReckoning = !constVelMode && !constPosMode; bool notDeadReckoning = !constVelMode && !constPosMode;
bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout; bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool gndOffsetValid = (imuSampleTime_ms - gndHgtValidTime_ms < 5000);
status = (!state.quat.is_nan()<<0 | // attitude valid (we need a better check) status = (!state.quat.is_nan()<<0 | // attitude valid (we need a better check)
(someHorizRefData && notDeadReckoning)<<1 | // horizontal velocity estimate valid (someHorizRefData && notDeadReckoning)<<1 | // horizontal velocity estimate valid
someVertRefData<<2 | // vertical velocity estimate valid someVertRefData<<2 | // vertical velocity estimate valid
((doingFlowNav || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning)<<3 | // relative horizontal position estimate valid ((doingFlowNav || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning)<<3 | // relative horizontal position estimate valid
(doingNormalGpsNav && notDeadReckoning)<<4 | // absolute horizontal position estimate valid (doingNormalGpsNav && notDeadReckoning)<<4 | // absolute horizontal position estimate valid
!hgtTimeout<<5 | // vertical position estimate valid !hgtTimeout<<5 | // vertical position estimate valid
!inhibitGndState<<6 | // terrain height estimate valid gndOffsetValid<<6 | // terrain height estimate valid
constPosMode<<7); // constant position mode constPosMode<<7); // constant position mode
} }

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -607,7 +607,7 @@ private:
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec) uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec) uint32_t flowMeaTime_ms; // time stamp from latest flow measurement (msec)
uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality. uint8_t flowQuality; // unsigned integer representing quality of optical flow data. 255 is maximum quality.
uint32_t rngMeaTime_ms; // time stamp from latest range measurement (msec) uint32_t gndHgtValidTime_ms; // time stamp from last terrain offset state update (msec)
Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period Vector3f omegaAcrossFlowTime; // body angular rates averaged across the optical flow sample period
Matrix3f Tnb_flow; // transformation matrix from nav to body axes at the middle of the optical flow sample period Matrix3f Tnb_flow; // transformation matrix from nav to body axes at the middle of the optical flow sample period
Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period Matrix3f Tbn_flow; // transformation matrix from body to nav axes at the middle of the optical flow sample period