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

ekf2: EV overhaul yaw and position fusion (#20501)

Browse Source 
- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality
This commit is contained in:
Daniel Agar
2022-12-20 10:23:56 -05:00
committed by GitHub
parent 20342216e2
commit 54a32eb2f7
34 changed files with 1019 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.hpp
+5 -5
View File
@@ -69,6 +69,7 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/estimator_bias.h>
#include <uORB/topics/estimator_bias3d.h>
#include <uORB/topics/estimator_event_flags.h>
#include <uORB/topics/estimator_gps_status.h>
#include <uORB/topics/estimator_innovations.h>
@@ -142,7 +143,7 @@ private:
void PublishBaroBias(const hrt_abstime &timestamp);
void PublishGnssHgtBias(const hrt_abstime &timestamp);
void PublishRngHgtBias(const hrt_abstime &timestamp);
void PublishEvHgtBias(const hrt_abstime &timestamp);
void PublishEvPosBias(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);
@@ -165,7 +166,7 @@ private:
void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -291,7 +292,7 @@ private:
float _last_baro_bias_published{};
float _last_gnss_hgt_bias_published{};
float _last_rng_hgt_bias_published{};
float _last_ev_hgt_bias_published{};
matrix::Vector3f _last_ev_bias_published{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
hrt_abstime _airspeed_validated_timestamp_last{0};
@@ -340,7 +341,7 @@ private:
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::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_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)};
@@ -350,7 +351,6 @@ private:
uORB::PublicationMulti<estimator_states_s> _estimator_states_pub{ORB_ID(estimator_states)};
uORB::PublicationMulti<estimator_status_flags_s> _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
uORB::PublicationMulti<vehicle_odometry_s> _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};