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

AttPosEKF: Make local_pos output Z ref pos relative

This commit is contained in:
Johan Jansen 2015-03-13 15:27:02 +01:00
parent 435d82dee2
commit 4a8f799e9e
2 changed files with 64 additions and 59 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

@ -193,6 +193,7 @@ private:
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
bool _baro_init;
float _baroAltRef;
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;

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

@ -173,6 +173,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_gpsIsGood(false),
_previousGPSTimestamp(0),
_baro_init(false),
_baroAltRef(0.0f),
_gps_initialized(false),
_filter_start_time(0),
_last_sensor_timestamp(0),
@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.y = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_local_pos.z = _ekf->states[9] - _baro_ref_offset;
_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
_local_pos.vx = _ekf->states[4];
_local_pos.vy = _ekf->states[5];
@ -1399,7 +1400,10 @@ void AttitudePositionEstimatorEKF::pollData()
}
baro_last = _baro.timestamp;
if(!_baro_init) {
_baro_init = true;
_baroAltRef = _baro.altitude;
}
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));