From c1e13e5afba251723cec51539ae08840d1fe3b29 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 11 Feb 2015 16:43:04 +0100 Subject: AttPosEKF: Fix GPS loss timeout not resetting properly --- .../ekf_att_pos_estimator_main.cpp | 39 +++++++++++----------- 1 file changed, 19 insertions(+), 20 deletions(-) (limited to 'src') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e02adb305..e06e174a5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1068,6 +1068,7 @@ void AttitudePositionEstimatorEKF::task_main() perf_count(_perf_gps); if (_gps.fix_type < 3) { + //Too poor GPS fix to use newDataGps = false; } else { @@ -1075,17 +1076,11 @@ void AttitudePositionEstimatorEKF::task_main() /* store time of valid GPS measurement */ _gps_start_time = hrt_absolute_time(); - /* check if we had a GPS outage for a long time */ - float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; - const float pos_reset_threshold = 5.0f; // seconds - /* timeout of 5 seconds */ - if (gps_elapsed > pos_reset_threshold) { - _ekf->ResetPosition(); - _ekf->ResetVelocity(); - _ekf->ResetStoredStates(); - } + //Calculate time since last good GPS fix + float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; + _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); /* fuse GPS updates */ @@ -1102,6 +1097,11 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + //Convert from global frame to local frame + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + // update LPF _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); @@ -1121,6 +1121,14 @@ void AttitudePositionEstimatorEKF::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + /* check if we had a GPS outage for a long time */ + /* timeout of 5 seconds */ + if (gps_elapsed > pos_reset_threshold) { + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + _ekf->ResetStoredStates(); + } + newDataGps = true; last_gps = _gps.timestamp_position; @@ -1275,10 +1283,6 @@ void AttitudePositionEstimatorEKF::task_main() double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; - // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame @@ -1383,19 +1387,14 @@ void AttitudePositionEstimatorEKF::task_main() _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; } - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step _ekf->FuseVelposNED(); -- cgit v1.2.3