diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 11:02:31 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 11:02:31 +0200 |
commit | 4585df1182083c39f2439bb7b88953dcc3575240 (patch) | |
tree | f15f32c6dc723d1f897b03bc3a28e0925f1395cc /src/modules/ekf_att_pos_estimator/estimator.cpp | |
parent | 1e80e624916a0eb1b13adccb4f700adeeee66bba (diff) | |
download | px4-firmware-4585df1182083c39f2439bb7b88953dcc3575240.tar.gz px4-firmware-4585df1182083c39f2439bb7b88953dcc3575240.tar.bz2 px4-firmware-4585df1182083c39f2439bb7b88953dcc3575240.zip |
Robustified filter init / sequencing
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 6 |
1 files changed, 4 insertions, 2 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ac9abf5ca..5de22fdae 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,11 +153,12 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), + refSet(false), storeIndex(0), gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f), + VtasMeas(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -1977,7 +1978,7 @@ void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { - staticMode = !(GPSstatus > GPS_FIX_2D); + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } } @@ -2485,6 +2486,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; + refSet = true; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); |