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

ekf2: publish GNSS, rng and EV height biases

Browse Source
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent 6833c7e311
commit aba2eac0df
7 changed files with 85 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.hpp
+13 -2
View File
@@ -67,7 +67,7 @@
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/estimator_baro_bias.h>
#include <uORB/topics/estimator_bias.h>
#include <uORB/topics/estimator_event_flags.h>
#include <uORB/topics/estimator_gps_status.h>
#include <uORB/topics/estimator_innovations.h>
@@ -137,6 +137,11 @@ private:
void PublishAidSourceStatus(const hrt_abstime &timestamp);
void PublishAttitude(const hrt_abstime &timestamp);
void PublishBaroBias(const hrt_abstime &timestamp);
void PublishGnssHgtBias(const hrt_abstime &timestamp);
void PublishRngHgtBias(const hrt_abstime &timestamp);
void PublishEvHgtBias(const hrt_abstime &timestamp);
estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
uint64_t timestamp, uint32_t device_id = 0);
void PublishEventFlags(const hrt_abstime &timestamp);
void PublishGlobalPosition(const hrt_abstime &timestamp);
void PublishGpsStatus(const hrt_abstime &timestamp);
@@ -269,6 +274,9 @@ private:
hrt_abstime _status_aux_vel_pub_last{0};
float _last_baro_bias_published{};
float _last_gnss_hgt_bias_published{};
float _last_rng_hgt_bias_published{};
float _last_ev_hgt_bias_published{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
hrt_abstime _airspeed_validated_timestamp_last{0};
@@ -312,7 +320,10 @@ private:
uint32_t _filter_information_event_changes{0};
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<estimator_baro_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
uORB::PublicationMulti<estimator_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
uORB::PublicationMulti<estimator_bias_s> _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub{ORB_ID(estimator_rng_hgt_bias)};
uORB::PublicationMulti<estimator_bias_s> _estimator_ev_hgt_bias_pub{ORB_ID(estimator_ev_hgt_bias)};
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};