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

ekf2: add OF estimator aid src status

Browse Source
This commit is contained in:
Daniel Agar
2022-10-15 13:19:12 -04:00
parent 75c63aee2a
commit 535415a537
15 changed files with 471 additions and 377 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+8 -2
View File
@@ -747,6 +747,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
// aux velocity
PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
// optical flow
PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
}
void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -1531,9 +1534,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
{
if (_ekf.getFlowCompensated().longerThan(0.f)) {
const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample;
if ((timestamp_sample != 0) && (timestamp_sample > _status_optical_flow_pub_last)) {
vehicle_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
flow_vel.timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample;
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);