fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update EKF auxiliary velocity interface

Browse Source
This commit is contained in:
kamilritz 2020-01-21 14:40:32 +01:00 committed by Mathieu Bresciani
parent 9d184ea3d6
commit 8d25dcd543
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf2/ekf2_main.cpp
View File

@ -1180,9 +1180,11 @@ void Ekf2::Run()
// we can only use the landing target if it has a fixed position and a valid velocity estimate
if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
matrix::Vector3f velocity { -landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f};
matrix::Vector3f variance {landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f};
_ekf.setAuxVelData(landing_target_pose.timestamp, velocity, variance);
auxVelSample auxvel_sample;
auxvel_sample.vel = matrix::Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f};
auxvel_sample.velVar = matrix::Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f};
auxvel_sample.time_us = landing_target_pose.timestamp;
_ekf.setAuxVelData(auxvel_sample);
}
}
}