From c2825f701a10d295399c727dc916ec53ab516714 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 23 Jun 2016 15:56:30 +0200 Subject: [PATCH] ekf_att_pos_estimator: fixed saving params when landed fixed logic such that parameters are saved when vehicle just landed. only save parameters once when state changed from in_air to landed. Signed-off-by: tumbili and bkueng --- .../ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h | 1 + .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index f7ea01bbd9..b66025c30b 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -224,6 +224,7 @@ private: bool _vibration_warning; ///< high vibration levels detected bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 + bool _was_landed; ///< indicates if was landed in previous iteration bool _newHgtData; bool _newAdsData; 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 7dc4afb77c..5b75ab2b12 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 @@ -216,6 +216,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _vibration_warning(false), _ekf_logging(true), _debug(0), + _was_landed(true), _newHgtData(false), _newAdsData(false), @@ -381,8 +382,8 @@ void AttitudePositionEstimatorEKF::vehicle_land_detected_poll() orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected); - // Save params on landed - if (!_vehicle_land_detected.landed) { + // Save params on landed and previously not landed + if (_vehicle_land_detected.landed && !_was_landed) { _mag_offset_x.set(_ekf->magBias.x); _mag_offset_x.commit(); _mag_offset_y.set(_ekf->magBias.y); @@ -390,6 +391,8 @@ void AttitudePositionEstimatorEKF::vehicle_land_detected_poll() _mag_offset_z.set(_ekf->magBias.z); _mag_offset_z.commit(); } + + _was_landed = _vehicle_land_detected.landed; } }