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

ekf2: enable multiple height sources fusion

Browse Source 
Instead of having a single height source fused into the EKF and the
other ones "waiting" for a failure or the primary sensor, fuse all
sources in EKF2 at the same time. To prevent the sources from fighting against each
other, the "primary" source is set as reference and the other ones are
running a bias estimator in order to make all the secondary height
sources converge to the primary one.

If the reference isn't available, another one is automatically selected
from a priority list. This secondary reference keeps its current bias
estimate but stops updating it in order to be the new reference as close
as possible to the primary one.
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent f5f31006a0
commit 8fd79688c0
17 changed files with 573 additions and 456 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+14 -9
View File
@@ -105,12 +105,12 @@ enum TerrainFusionMask : uint8_t {
TerrainFuseOpticalFlow = (1 << 1)
};
enum VerticalHeightSensor : uint8_t {
// Integer definitions for vdist_sensor_type
BARO = 0, ///< Use baro height
GPS = 1, ///< Use GPS height
RANGE = 2, ///< Use range finder height
EV = 3 ///< Use external vision
enum HeightSensor : uint8_t {
BARO = 0,
GNSS = 1,
RANGE = 2,
EV = 3,
UNKNOWN = 4
};
enum SensorFusionMask : uint16_t {
@@ -123,7 +123,11 @@ enum SensorFusionMask : uint16_t {
USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind
ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available
USE_EXT_VIS_VEL = (1<<8) ///< set to true to use external vision velocity data
USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
USE_BARO_HGT = (1<<9), ///< set to true to use baro data
USE_GPS_HGT = (1<<10), ///< set to true to use GPS height data
USE_RNG_HGT = (1<<11), ///< set to true to use range height data
USE_EXT_VIS_HGT = (1<<12) ///< set to true to use external vision height data
};
struct gpsMessage {
@@ -247,8 +251,9 @@ struct parameters {
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
// measurement source control
int32_t fusion_mode{SensorFusionMask::USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used
int32_t vdist_sensor_type{VerticalHeightSensor::BARO}; ///< selects the primary source for height data
int32_t fusion_mode{SensorFusionMask::USE_GPS |
SensorFusionMask::USE_BARO_HGT}; ///< bitmasked integer that selects which aiding sources will be used
int32_t height_sensor_ref{HeightSensor::BARO};
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator