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.