Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:07:39 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

ekf2: publish vertical position derivative

Browse Source
This commit is contained in:
Paul Riseborough
2017-04-27 10:51:50 +10:00
committed by Dennis Mannhart
parent 2e008dfbc4
commit 4b7ae78fda
1 changed files with 5 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+5
View File
@@ -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.