From 28017105f456e27034771fa3a7f3befb1b4a1a34 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 27 Apr 2017 11:24:51 +1000 Subject: [PATCH] position_estimator_inav: publish vertical position derivative --- .../position_estimator_inav_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 0997126a51..6ebd2baf64 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1346,6 +1346,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.evh = 0.0f; local_pos.evv = 0.0f; + // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity + local_pos.z_deriv = z_est[1]; + if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = dist_ground; local_pos.dist_bottom_rate = - z_est[1]; @@ -1371,6 +1374,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.vel_e = local_pos.vy; global_pos.vel_d = local_pos.vz; + // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity + global_pos.pos_d_deriv = local_pos.vz; + global_pos.yaw = local_pos.yaw; global_pos.eph = eph;