dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <roman@px4.io> and bkueng <beat-kueng@gmx.net>
This commit is contained in:
tumbili 2016-06-23 15:56:30 +02:00
parent 8026273cb0
commit c2825f701a
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -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;

7
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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;
}
}