diff options
4 files changed, 18 insertions, 22 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2bbc6c916..1a190bf40 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -328,6 +328,9 @@ GPS::task_main() //DEBUG BEGIN: Disable GPS using aux1 orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.fix_type = 0; _report_gps_pos.satellites_used = 0; 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 936092195..8ec7c43a1 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 @@ -230,7 +230,7 @@ private: float _baro_alt_filt; float _covariancePredictionDt; ///< time lapsed since last covariance prediction bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use - uint64_t _lastGPSTimestamp; ///< Timestamp of last good GPS fix we have received + uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; bool _gps_initialized; hrt_abstime _filter_start_time; @@ -372,6 +372,11 @@ private: **/ void initializeGPS(); + /** + * @brief + * Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate + * flags to true (e.g newDataGps) + **/ void pollData(); }; @@ -451,7 +456,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _baro_alt_filt(0.0f), _covariancePredictionDt(0.0f), _gpsIsGood(false), - _lastGPSTimestamp(0), + _previousGPSTimestamp(0), _baro_init(false), _gps_initialized(false), _filter_start_time(0), @@ -1209,18 +1214,6 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (fuseGPS && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED - float gps_dt = (_gps.timestamp_position - _lastGPSTimestamp) / 1e6f; - - // Calculate acceleration predicted by GPS velocity change - if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { - - _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; - _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; - _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; - } - // set fusion flags _ekf->fuseVelData = true; _ekf->fusePosData = true; @@ -1522,8 +1515,6 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->VtasMeas = _airspeed.true_airspeed_m_s; } - //Calculate time since last good GPS fix - const float dtGoodGPS = hrt_elapsed_time(&_lastGPSTimestamp) / 1e6f; orb_check(_gps_sub, &_newDataGps); if (_newDataGps) { @@ -1546,7 +1537,11 @@ void AttitudePositionEstimatorEKF::pollData() } if (_gpsIsGood) { - _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - _lastGPSTimestamp) / 1e6f, 0.01f, POS_RESET_THRESHOLD)); + + //Calculate time since last good GPS fix + const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + + _ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD)); /* fuse GPS updates */ @@ -1569,7 +1564,7 @@ void AttitudePositionEstimatorEKF::pollData() float posNED[3] = {0.0f, 0.0f, 0.0f}; _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]; + _ekf->posNE[1] = posNED[1]; if (dtGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); @@ -1597,7 +1592,7 @@ void AttitudePositionEstimatorEKF::pollData() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); - _lastGPSTimestamp = _gps.timestamp_position; + _previousGPSTimestamp = _gps.timestamp_position; } else { //Too poor GPS fix to use @@ -1607,7 +1602,7 @@ 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(dtGoodGPS > POS_RESET_THRESHOLD) { + else if( (static_cast<float>(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) { _gpsIsGood = false; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 47706c37b..e912e699e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -111,7 +111,6 @@ AttPosEKF::AttPosEKF() : innovVelPos{}, varInnovVelPos{}, velNED{}, - accelGPSNED{}, posNE{}, hgtMea(0.0f), baroHgtOffset(0.0f), diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 5de9d4c5a..19ef52145 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -161,7 +161,6 @@ public: float varInnovVelPos[6]; // innovation variance output float velNED[3]; // North, East, Down velocity obs (m/s) - float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) float baroHgtOffset; ///< the baro (weather) offset from normalized altitude |