From 4b7ae78fdab1418768b734daf247fe265ae3661a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 27 Apr 2017 10:51:50 +1000 Subject: [PATCH] ekf2: publish vertical position derivative --- src/modules/ekf2/ekf2_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 94bdde8190..24da921cb2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -789,6 +789,9 @@ void Ekf2::task_main() float velocity[3]; _ekf.get_velocity(velocity); + float pos_d_deriv; + _ekf.get_pos_d_deriv(&pos_d_deriv); + float gyro_rad[3]; { @@ -911,6 +914,7 @@ void Ekf2::task_main() lpos.vx = velocity[0]; lpos.vy = velocity[1]; lpos.vz = velocity[2]; + lpos.z_deriv = pos_d_deriv; // vertical position time derivative (m/s) // TODO: better status reporting lpos.xy_valid = _ekf.local_position_is_valid() && !_vel_innov_preflt_fail; @@ -978,6 +982,7 @@ void Ekf2::task_main() global_pos.vel_n = velocity[0]; // Ground north velocity, m/s global_pos.vel_e = velocity[1]; // Ground east velocity, m/s global_pos.vel_d = velocity[2]; // Ground downside velocity, m/s + global_pos.pos_d_deriv = pos_d_deriv; // vertical position time derivative, m/s global_pos.yaw = euler.psi(); // Yaw in radians -PI..+PI.