diff options
| author | Johan Jansen <jnsn.johan@gmail.com> | 2015-03-12 17:28:21 +0100 |
|---|---|---|
| committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-03-12 17:28:21 +0100 |
| commit | 211760e3e3e5af3ca8c9d0f96368d39d156fbad3 (patch) | |
| tree | 3fe851cfd61cbb53a2d3d796c0a0774070696d49 /src/modules/ekf_att_pos_estimator | |
| parent | 67695f191ea6f528971c6d2da3dcac66540d7be9 (diff) | |
| download | px4-firmware-211760e3e3e5af3ca8c9d0f96368d39d156fbad3.tar.gz px4-firmware-211760e3e3e5af3ca8c9d0f96368d39d156fbad3.tar.bz2 px4-firmware-211760e3e3e5af3ca8c9d0f96368d39d156fbad3.zip | |
AttPosEKF: Fix 5Hz sawtooth oscilation in XY position estimate
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h | 1 | ||||
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 14 |
2 files changed, 4 insertions, 11 deletions
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 |