From 211760e3e3e5af3ca8c9d0f96368d39d156fbad3 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Mar 2015 17:28:21 +0100 Subject: AttPosEKF: Fix 5Hz sawtooth oscilation in XY position estimate --- .../ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h | 1 - .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 14 ++++---------- 2 files changed, 4 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index ec9efe8ee..712b4a17e 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -208,7 +208,6 @@ private: bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 - bool _newDataGps; bool _newHgtData; bool _newAdsData; bool _newDataMag; 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 dfe3f5357..3e007a9f9 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 @@ -188,7 +188,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _ekf_logging(true), _debug(0), - _newDataGps(false), _newHgtData(false), _newAdsData(false), _newDataMag(false), @@ -668,7 +667,7 @@ void AttitudePositionEstimatorEKF::task_main() } //Run EKF data fusion steps - updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); //Publish attitude estimations publishAttitude(); @@ -1267,10 +1266,10 @@ void AttitudePositionEstimatorEKF::pollData() } - orb_check(_gps_sub, &_newDataGps); - - if (_newDataGps) { + bool gps_update; + orb_check(_gps_sub, &gps_update); + if (gps_update) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -1349,9 +1348,6 @@ void AttitudePositionEstimatorEKF::pollData() _previousGPSTimestamp = _gps.timestamp_position; - } else { - //Too poor GPS fix to use - _newDataGps = false; } } @@ -1415,8 +1411,6 @@ void AttitudePositionEstimatorEKF::pollData() } perf_count(_perf_baro); - - _newHgtData = true; } //Update Magnetometer -- cgit v1.2.3