Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Track baro height observations pre-arm

Browse Source 
This prevents copter from failing the pre-arm height discrepancy test
This commit is contained in:
priseborough 2014-03-23 15:04:20 +11:00 committed by Andrew Tridgell
parent 7780d55788
commit 7ae86b3979
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -641,9 +641,6 @@ void NavEKF::SelectVelPosFusion()
fusePosData = false; fusePosData = false;
} }
// check for and read new height data
readHgtData();
// command fusion of height data // command fusion of height data
if (newDataHgt) if (newDataHgt)
{ {
@ -660,7 +657,7 @@ void NavEKF::SelectVelPosFusion()
} else { } else {
// in static mode we only fuse position and height to improve long term numerical stability // in static mode we only fuse position and height to improve long term numerical stability
// and only when the rate of change of velocity is less than 0.5g. This prevents attitude errors // and only when the rate of change of velocity is less than 0.5g. This prevents attitude errors
// due to launch acceleration // due to launch acceleration. position is assumed to be zero.
fuseVelData = false; fuseVelData = false;
if (accNavMag < 4.9f) { if (accNavMag < 4.9f) {
fusePosData = true; fusePosData = true;
@ -671,6 +668,9 @@ void NavEKF::SelectVelPosFusion()
} }
} }
// check for and read new height data
readHgtData();
// perform fusion as commanded, and in accordance with specified time intervals // perform fusion as commanded, and in accordance with specified time intervals
if (fuseVelData || fusePosData || fuseHgtData) { if (fuseVelData || fusePosData || fuseHgtData) {
// skip fusion as required to maintain ~dtVelPos time interval between corrections // skip fusion as required to maintain ~dtVelPos time interval between corrections
@ -1555,14 +1555,14 @@ void NavEKF::FuseVelPosNED()
if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS; if (useAirspeed()) gpsRetryTime = _gpsRetryTimeUseTAS;
else gpsRetryTime = _gpsRetryTimeNoTAS; else gpsRetryTime = _gpsRetryTimeNoTAS;
// form the observation vector and zero observations if in static mode // form the observation vector and zero velocity and horizontal position observations if in static mode
if (~staticMode) { if (~staticMode) {
for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
observation[5] = -hgtMea;
} else { } else {
for (uint8_t i=0; i<=5; i++) observation[i] = 0.0f; for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f;
} }
observation[5] = -hgtMea;
// calculate additional error in GPS velocity caused by manoeuvring // calculate additional error in GPS velocity caused by manoeuvring
NEvelErr = _gpsNEVelVarAccScale * accNavMag; NEvelErr = _gpsNEVelVarAccScale * accNavMag;