fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 23:59:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move AuxVelSample update to UpdateAuxVelSample()

Browse Source
This commit is contained in:
Daniel Agar 2020-10-31 15:16:14 -04:00
parent 651e292d42
commit 6b30f01daf
2 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/modules/ekf2/EKF2.cpp
View File

@ -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 &timestamp)
{
/* Check and save learned magnetometer bias estimates */

1
src/modules/ekf2/EKF2.hpp
View File

@ -147,6 +147,7 @@ private:
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateMagCalibration(const hrt_abstime &timestamp);