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

ekf2: add kconfig option to enable/disable external vision fusion

Browse Source
This commit is contained in:
Daniel Agar
2023-03-20 10:12:17 -04:00
parent d47f96f1a5
commit 4363b09421
17 changed files with 209 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+7 -2
View File
@@ -238,6 +238,7 @@ struct flowSample {
uint8_t quality{}; ///< quality indicator between 0 and 255
};
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
struct extVisionSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector3f pos{}; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
@@ -251,6 +252,7 @@ struct extVisionSample {
uint8_t reset_counter{};
int8_t quality{}; ///< quality indicator between 0 and 100
};
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_DRAG_FUSION)
struct dragSample {
@@ -299,7 +301,6 @@ struct parameters {
int32_t baro_ctrl{1};
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
int32_t rng_ctrl{RngCtrl::CONDITIONAL};
int32_t ev_ctrl{0};
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
@@ -311,7 +312,6 @@ struct parameters {
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
// input noise
float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
@@ -395,7 +395,11 @@ struct parameters {
float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// vision position fusion
int32_t ev_ctrl{0};
float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec)
float ev_pos_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m)
float ev_att_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
@@ -403,6 +407,7 @@ struct parameters {
float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
#endif // CONFIG_EKF2_EXTERNAL_VISION
// gravity fusion
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)