diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 438d11917c..aa8aab94dd 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -606,22 +606,7 @@ void EKF2::Run() } } - // use the landing target pose estimate as another source of velocity data - if (_landing_target_pose_sub.updated()) { - landing_target_pose_s landing_target_pose; - - if (_landing_target_pose_sub.copy(&landing_target_pose)) { - // 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 - auxVelSample auxvel_sample {}; - auxvel_sample.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f}; - auxvel_sample.velVar = 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); - } - } - } + UpdateAuxVelSample(ekf2_timestamps); // run the EKF update and output perf_begin(_ekf_update_perf); @@ -1367,6 +1352,26 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } } +void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) +{ + // EKF auxillary velocity sample + // - use the landing target pose estimate as another source of velocity data + landing_target_pose_s landing_target_pose; + + if (_landing_target_pose_sub.update(&landing_target_pose)) { + // 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 + auxVelSample auxvel_sample{ + .vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f}, + .velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f}, + .time_us = landing_target_pose.timestamp, + }; + _ekf.setAuxVelData(auxvel_sample); + } + } +} + void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { /* Check and save learned magnetometer bias estimates */ diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 7630ee7f89..aa51caff39 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -147,6 +147,7 @@ private: void PublishYawEstimatorStatus(const hrt_abstime ×tamp); void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); + void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagCalibration(const hrt_abstime ×tamp);