From 35e0faa48aba9c951e8e00ebe5bc132277e76bda Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 3 Mar 2015 13:28:55 +0100 Subject: AttPosEKF: Fix dead-reckoning for global position estimates without GPS --- .../ekf_att_pos_estimator_main.cpp | 51 ++++++++++++++++------ 1 file changed, 38 insertions(+), 13 deletions(-) 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 7de44c75b..ed140a728 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 @@ -670,7 +670,7 @@ void AttitudePositionEstimatorEKF::task_main() publishLocalPosition(); //Publish Global Position, but only if it's any good - if(_gps_initialized && _gpsIsGood) + if(_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { publishGlobalPosition(); } @@ -1265,24 +1265,32 @@ void AttitudePositionEstimatorEKF::pollData() if (_gpsIsGood) { //Calculate time since last good GPS fix - const float dtGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; - _ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD)); - - /* fuse GPS updates */ + //Stop dead-reckoning mode + if(_global_pos.dead_reckoning) { + mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning"); + } + _global_pos.dead_reckoning = false; - //_gps.timestamp / 1e3; + //Fetch new GPS data _ekf->GPSstatus = _gps.fix_type; _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + if(_previousGPSTimestamp != 0) { + //Calculate average time between GPS updates + _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); + + // update LPF + _gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + } + //check if we had a GPS outage for a long time if(_gps_initialized) { @@ -1292,15 +1300,12 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->posNE[0] = posNED[0]; _ekf->posNE[1] = posNED[1]; - if (dtGoodGPS > POS_RESET_THRESHOLD) { + if (dtLastGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); _ekf->ResetVelocity(); } } - // update LPF - _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { @@ -1327,8 +1332,28 @@ void AttitudePositionEstimatorEKF::pollData() // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - else if( (static_cast(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) { + const float dtLastGoodGPS = static_cast(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + if(dtLastGoodGPS >= POS_RESET_THRESHOLD) { + + if(_global_pos.dead_reckoning) { + mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout"); + } + _gpsIsGood = false; + _global_pos.dead_reckoning = false; + } + + //If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds) + else if(dtLastGoodGPS >= 0.5f) { + if(_armed.armed) { + if(!_global_pos.dead_reckoning) { + mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled"); + } + _global_pos.dead_reckoning = true; + } + else { + _global_pos.dead_reckoning = false; + } } //Update barometer -- cgit v1.2.3