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 | |
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')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 6 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.h | 1 | ||||
-rw-r--r-- | src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 13 |
3 files changed, 14 insertions, 6 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)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 5e60e506f..e118ae573 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -211,6 +211,7 @@ public: double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) + bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad7cb3687..19111a9b4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -192,6 +192,7 @@ private: bool _initialized; bool _gps_initialized; + uint64_t _gps_start_time; int _mavlink_fd; @@ -720,6 +721,9 @@ FixedwingEstimator::task_main() } else { + /* store time of valid GPS measurement */ + _gps_start_time = hrt_absolute_time(); + /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); @@ -859,7 +863,7 @@ FixedwingEstimator::task_main() } // XXX trap for testing - if (check == 1 || check == 2) { + if (check == 1) { errx(1, "NUMERIC ERROR IN FILTER"); } @@ -907,7 +911,7 @@ FixedwingEstimator::task_main() // XXX we rather want to check all updated - if (hrt_elapsed_time(&start_time) > 100000) { + if (hrt_elapsed_time(&_gps_start_time) > 50000) { bool home_set; orb_check(_home_sub, &home_set); @@ -920,7 +924,6 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); - warnx("HOME SET"); double lat = home.lat; double lon = home.lon; @@ -942,7 +945,9 @@ FixedwingEstimator::task_main() // XXX this is not multithreading safe map_projection_init(lat, lon); - mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); + warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); _gps_initialized = true; |