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

ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)

Browse Source
This commit is contained in:
Daniel Agar
2023-03-21 11:25:34 -04:00
parent 0784901a66
commit 32a5bd32ad
17 changed files with 343 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+23 -11
View File
@@ -108,10 +108,12 @@ enum MagFuseType : uint8_t {
NONE = 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
};
#if defined(CONFIG_EKF2_RANGE_FINDER)
enum TerrainFusionMask : uint8_t {
TerrainFuseRangeFinder = (1 << 0),
TerrainFuseOpticalFlow = (1 << 1)
};
#endif // CONFIG_EKF2_RANGE_FINDER
enum HeightSensor : uint8_t {
BARO = 0,
@@ -300,9 +302,6 @@ struct parameters {
int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
int32_t baro_ctrl{1};
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
int32_t rng_ctrl{RngCtrl::CONDITIONAL};
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
@@ -310,7 +309,6 @@ struct parameters {
float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
float range_delay_ms{5.0f}; ///< range finder 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)
@@ -324,10 +322,6 @@ struct parameters {
float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
// initialization errors
float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
@@ -379,7 +373,14 @@ struct parameters {
const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec)
#endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_RANGE_FINDER)
// range finder fusion
int32_t rng_ctrl{RngCtrl::CONDITIONAL};
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
float range_noise{0.1f}; ///< observation noise for range finder measurements (m)
float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD)
float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz))
@@ -394,6 +395,14 @@ 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
Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m)
float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// vision position fusion
int32_t ev_ctrl{0};
@@ -406,6 +415,8 @@ 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))
Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
#endif // CONFIG_EKF2_EXTERNAL_VISION
// gravity fusion
@@ -419,6 +430,8 @@ struct parameters {
float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor
float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
#endif // CONFIG_EKF2_OPTICAL_FLOW
// these parameters control the strictness of GPS quality checks used to determine if the GPS is
@@ -435,9 +448,6 @@ struct parameters {
// XYZ offset of sensors in body axes (m)
Vector3f imu_pos_body{}; ///< xyz position of IMU in body frame (m)
Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m)
Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m)
Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m)
Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m)
// accel bias learning control
float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2)
@@ -617,6 +627,7 @@ union ekf_solution_status_u {
uint16_t value;
};
#if defined(CONFIG_EKF2_RANGE_FINDER)
union terrain_fusion_status_u {
struct {
bool range_finder : 1; ///< 0 - true if we are fusing range finder data
@@ -624,6 +635,7 @@ union terrain_fusion_status_u {
} flags;
uint8_t value;
};
#endif // CONFIG_EKF2_RANGE_FINDER
// define structure used to communicate information events
union information_event_status_u {