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

ekf2: estimator aid source status (starting with fake position)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-23 16:21:49 -04:00
parent 79a34b5aed
commit 1ae467e9cd
13 changed files with 287 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.hpp
+20
View File
@@ -135,6 +135,7 @@ private:
void Run() override;
void PublishAidSourceStatus(const hrt_abstime &timestamp);
void PublishAttitude(const hrt_abstime &timestamp);
void PublishBaroBias(const hrt_abstime &timestamp);
void PublishEventFlags(const hrt_abstime &timestamp);
@@ -167,6 +168,21 @@ private:
void UpdateGyroCalibration(const hrt_abstime &timestamp);
void UpdateMagCalibration(const hrt_abstime &timestamp);
// publish helper for estimator_aid_source topics
template <typename T>
void PublishAidSourceStatus(const T &status, hrt_abstime &status_publish_last, uORB::PublicationMulti<T> &pub)
{
if (status.timestamp_sample > status_publish_last) {
// publish if updated
T status_out{status};
status_out.estimator_instance = _instance;
status_out.timestamp = hrt_absolute_time();
pub.publish(status_out);
// record timestamp sample
status_publish_last = status.timestamp_sample;
}
}
/*
* Calculate filtered WGS84 height from estimated AMSL height
@@ -239,6 +255,8 @@ private:
hrt_abstime _last_sensor_bias_published{0};
hrt_abstime _last_gps_status_published{0};
hrt_abstime _status_fake_pos_pub_last{0};
float _last_baro_bias_published{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
@@ -298,6 +316,8 @@ private:
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
// publications with topic dependent on multi-mode
uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub;
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;