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

AP_NavEKF: Track baro alt when pre-armed

Browse Source 
This will help prevent spurious alt disparity warning messages for copter
This commit is contained in:
priseborough 2014-10-12 22:17:38 +11:00 committed by Randy Mackay
parent f866bf979e
commit 2bc74934b8
1 changed files with 16 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -702,23 +702,6 @@ void NavEKF::SelectVelPosFusion()
fusePosData = false;
}
// check for and read new height data
readHgtData();
// command fusion of height data
if (newDataHgt)
{
// reset data arrived flag
newDataHgt = false;
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
hgtUpdateCount = 0;
// enable fusion
fuseHgtData = true;
} else {
fuseHgtData = false;
}
} else {
// in static mode use synthetic position measurements set to zero
// only fuse synthetic measurements when rate of change of velocity is less than 0.5g to reduce attitude errors due to launch acceleration
@ -729,7 +712,23 @@ void NavEKF::SelectVelPosFusion()
fusePosData = false;
}
fuseVelData = false;
}
// check for and read new height data
readHgtData();
// command fusion of height data
if (newDataHgt)
{
// reset data arrived flag
newDataHgt = false;
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta));
hgtUpdateCount = 0;
// enable fusion
fuseHgtData = true;
} else {
fuseHgtData = false;
}
// perform fusion