From 7aec94d4d6d5d1e9edbc86740d383c45a3befc66 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 27 Apr 2017 11:23:56 +1000 Subject: [PATCH] ekf_att_pos_estimator: publish vertical position derivative --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a0d777a11a..c027dbeb2a 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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();