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

ekf_att_pos_estimator: publish vertical position derivative

This commit is contained in:
Paul Riseborough 2017-04-27 11:23:56 +10:00 committed by Dennis Mannhart
parent 4b7ae78fda
commit 7aec94d4d6
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -948,6 +948,9 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.vy = _ekf->states[5];
_local_pos.vz = _ekf->states[6];
// this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
_local_pos.z_deriv = _ekf->states[6];
_local_pos.xy_valid = _gps_initialized && _gpsIsGood;
_local_pos.z_valid = true;
_local_pos.v_xy_valid = _gps_initialized && _gpsIsGood;
@ -1016,11 +1019,13 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
} else {
_global_pos.vel_d = 0.0f;
}
// this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
_global_pos.pos_d_deriv = _global_pos.vel_d;
/* terrain altitude */
if (_terrain_estimator->is_valid()) {
_global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground();