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

AP_NavEKF: Prevent load leveling from dropping flow measurements

Browse Source
This commit is contained in:
priseborough 2015-01-07 12:34:18 +11:00 committed by Andrew Tridgell
parent 873860e810
commit 92bb75a635
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -394,7 +394,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high. DCM33FlowMin(0.71f), // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
fScaleFactorPnoise(1e-10f), // Process noise added to focal length scale factor state variance at each time step fScaleFactorPnoise(1e-10f), // Process noise added to focal length scale factor state variance at each time step
flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec) flowTimeDeltaAvg_ms(100), // average interval between optical flow measurements (msec)
flowIntervalMax_ms(200) // maximum allowable time between flow fusion events flowIntervalMax_ms(100) // maximum allowable time between flow fusion events
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")), ,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@ -910,9 +910,9 @@ void NavEKF::SelectFlowFusion()
// Perform tilt check // Perform tilt check
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin); bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin);
// if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading // if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading
bool flowUseTimeout = ((imuSampleTime_ms - prevFlowUseTime_ms) >= flowIntervalMax_ms); bool flowFuseNow = ((imuSampleTime_ms - flowValidMeaTime_ms) >= flowIntervalMax_ms/2);
// check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature. // check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature.
bool delayFusion = ((covPredStep || magFusePerformed) && !(flowUseTimeout || inhibitLoadLeveling)); bool delayFusion = ((covPredStep || magFusePerformed) && !(flowFuseNow || inhibitLoadLeveling));
// if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in position hold mode // if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in position hold mode
if (!flowDataValid && !constPosMode && PV_AidingMode == AID_RELATIVE) { if (!flowDataValid && !constPosMode && PV_AidingMode == AID_RELATIVE) {