forked from Archive/PX4-Autopilot
ekf2: add dedicated EKF2_CONFIG_TERRAIN in kconfig
- new estimator_aid_src_terrain_range_finder for HAGL RNG
This commit is contained in:
Daniel Agar
parent
845b01a00d
commit
10b54d08fc
|
|
@ -22,3 +22,4 @@ bool fused # true if the sample was successfully fused
|
|||
# TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
|
||||
# TOPICS estimator_aid_src_fake_hgt
|
||||
# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
|
||||
# TOPICS estimator_aid_src_terrain_range_finder
|
||||
|
|
|
|||
|
|
@ -139,7 +139,6 @@ if(CONFIG_EKF2_RANGE_FINDER)
|
|||
EKF/range_finder_consistency_check.cpp
|
||||
EKF/range_height_control.cpp
|
||||
EKF/sensor_range_finder.cpp
|
||||
EKF/terrain_estimator.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
|
|
@ -147,6 +146,10 @@ if(CONFIG_EKF2_SIDESLIP)
|
|||
list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_TERRAIN)
|
||||
list(APPEND EKF_SRCS EKF/terrain_estimator.cpp)
|
||||
endif()
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__ekf2
|
||||
MAIN ekf2
|
||||
|
|
|
|||
|
|
@ -104,7 +104,6 @@ if(CONFIG_EKF2_RANGE_FINDER)
|
|||
range_finder_consistency_check.cpp
|
||||
range_height_control.cpp
|
||||
sensor_range_finder.cpp
|
||||
terrain_estimator.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
|
|
@ -112,6 +111,10 @@ if(CONFIG_EKF2_SIDESLIP)
|
|||
list(APPEND EKF_SRCS sideslip_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_TERRAIN)
|
||||
list(APPEND EKF_SRCS terrain_estimator.cpp)
|
||||
endif()
|
||||
|
||||
add_library(ecl_EKF
|
||||
${EKF_SRCS}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -105,12 +105,12 @@ enum MagFuseType : uint8_t {
|
|||
NONE = 5 ///< Do not use magnetometer under any circumstance..
|
||||
};
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
enum TerrainFusionMask : uint8_t {
|
||||
TerrainFuseRangeFinder = (1 << 0),
|
||||
TerrainFuseOpticalFlow = (1 << 1)
|
||||
};
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
enum HeightSensor : uint8_t {
|
||||
BARO = 0,
|
||||
|
|
@ -362,18 +362,27 @@ 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_TERRAIN)
|
||||
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
|
||||
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
|
||||
|
||||
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_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m)
|
||||
#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#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))
|
||||
float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m)
|
||||
float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
|
||||
float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m)
|
||||
const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
|
||||
|
|
@ -385,11 +394,6 @@ struct parameters {
|
|||
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)
|
||||
|
|
@ -625,7 +629,7 @@ union ekf_solution_status_u {
|
|||
uint16_t value;
|
||||
};
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
union terrain_fusion_status_u {
|
||||
struct {
|
||||
bool range_finder : 1; ///< 0 - true if we are fusing range finder data
|
||||
|
|
@ -633,7 +637,7 @@ union terrain_fusion_status_u {
|
|||
} flags;
|
||||
uint8_t value;
|
||||
};
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// define structure used to communicate information events
|
||||
union information_event_status_u {
|
||||
|
|
|
|||
|
|
@ -189,10 +189,10 @@ bool Ekf::update()
|
|||
// control fusion of observation data
|
||||
controlFusionModes(imu_sample_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// run a separate filter for terrain estimation
|
||||
runTerrainEstimator(imu_sample_delayed);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
|
||||
|
||||
|
|
@ -229,10 +229,10 @@ bool Ekf::initialiseFilter()
|
|||
// initialise the state covariance matrix now we have starting values for all the states
|
||||
initialiseCovariance();
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// Initialise the terrain estimator
|
||||
initHagl();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// reset the output predictor state history to match the EKF initial values
|
||||
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
|
||||
|
|
|
|||
|
|
@ -95,23 +95,7 @@ public:
|
|||
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
|
||||
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range height
|
||||
const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
|
||||
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
|
||||
|
||||
void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
|
||||
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
|
||||
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
|
||||
|
||||
void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
|
||||
void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
|
||||
void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
|
||||
|
||||
void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
|
||||
void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
|
||||
void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// terrain estimate
|
||||
bool isTerrainEstimateValid() const;
|
||||
|
||||
|
|
@ -125,13 +109,64 @@ public:
|
|||
|
||||
// get the terrain variance
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
|
||||
|
||||
void getHaglInnov(float &hagl_innov) const { hagl_innov = _aid_src_terrain_range_finder.innovation; }
|
||||
void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; }
|
||||
void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _aid_src_terrain_range_finder.test_ratio; }
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
|
||||
|
||||
void getTerrainFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
|
||||
flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
|
||||
}
|
||||
|
||||
void getTerrainFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
|
||||
}
|
||||
|
||||
void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range height
|
||||
const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
|
||||
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
|
||||
|
||||
void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
|
||||
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
|
||||
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
|
||||
|
||||
void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
|
||||
void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
|
||||
void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
|
||||
|
||||
void getFlowInnov(float flow_innov[2]) const;
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const;
|
||||
void getFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_optical_flow.innovation[0];
|
||||
flow_innov[1] = _aid_src_optical_flow.innovation[1];
|
||||
}
|
||||
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
|
||||
}
|
||||
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
|
||||
|
||||
const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
|
||||
|
|
@ -142,12 +177,6 @@ public:
|
|||
|
||||
const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
|
||||
const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
|
||||
|
||||
void getTerrainFlowInnov(float flow_innov[2]) const;
|
||||
void getTerrainFlowInnovVar(float flow_innov_var[2]) const;
|
||||
void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
|
||||
|
||||
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
|
|
@ -598,30 +627,33 @@ private:
|
|||
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
estimator_aid_source1d_s _aid_src_rng_hgt{};
|
||||
|
||||
HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
|
||||
|
||||
float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m)
|
||||
float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
|
||||
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
|
||||
|
||||
uint64_t _time_last_healthy_rng_data{0};
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// Terrain height state estimation
|
||||
float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
|
||||
float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2)
|
||||
uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset
|
||||
uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator
|
||||
terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
|
||||
|
||||
terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
|
||||
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
estimator_aid_source1d_s _aid_src_terrain_range_finder{};
|
||||
uint64_t _time_last_healthy_rng_data{0};
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
estimator_aid_source1d_s _aid_src_rng_hgt{};
|
||||
HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
estimator_aid_source2d_s _aid_src_optical_flow{};
|
||||
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
|
||||
|
||||
// optical flow processing
|
||||
Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
|
||||
|
|
@ -635,7 +667,6 @@ private:
|
|||
Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
|
||||
|
||||
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
|
||||
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
estimator_aid_source1d_s _aid_src_baro_hgt{};
|
||||
|
|
@ -867,12 +898,7 @@ private:
|
|||
void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
|
||||
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range height
|
||||
void controlRangeHeightFusion();
|
||||
bool isConditionalRangeAidSuitable();
|
||||
void stopRngHgtFusion();
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// terrain vertical position estimator
|
||||
void initHagl();
|
||||
void runTerrainEstimator(const imuSample &imu_delayed);
|
||||
|
|
@ -880,16 +906,33 @@ private:
|
|||
|
||||
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
|
||||
|
||||
void controlHaglFakeFusion();
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// update the terrain vertical position estimate using a height above ground measurement from the range finder
|
||||
void controlHaglRngFusion();
|
||||
void fuseHaglRng();
|
||||
void startHaglRngFusion();
|
||||
void resetHaglRngIfNeeded();
|
||||
void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
|
||||
void fuseHaglRng(estimator_aid_source1d_s &aid_src);
|
||||
void resetHaglRng();
|
||||
void stopHaglRngFusion();
|
||||
float getRngVar();
|
||||
float getRngVar() const;
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
void controlHaglFakeFusion();
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// update the terrain vertical position estimate using an optical flow measurement
|
||||
void controlHaglFlowFusion();
|
||||
void resetHaglFlow();
|
||||
void stopHaglFlowFusion();
|
||||
void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range height
|
||||
void controlRangeHeightFusion();
|
||||
bool isConditionalRangeAidSuitable();
|
||||
void stopRngHgtFusion();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
|
@ -912,13 +955,6 @@ private:
|
|||
void fuseOptFlow();
|
||||
float predictFlowRange();
|
||||
Vector2f predictFlowVelBody();
|
||||
|
||||
// update the terrain vertical position estimate using an optical flow measurement
|
||||
void controlHaglFlowFusion();
|
||||
void startHaglFlowFusion();
|
||||
void resetHaglFlow();
|
||||
void stopHaglFlowFusion();
|
||||
void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
|
|
|||
|
|
@ -363,32 +363,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
|
|||
}
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void Ekf::getFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_optical_flow.innovation[0];
|
||||
flow_innov[1] = _aid_src_optical_flow.innovation[1];
|
||||
}
|
||||
|
||||
void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
|
||||
}
|
||||
|
||||
void Ekf::getTerrainFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
|
||||
flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
|
||||
}
|
||||
|
||||
void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||
{
|
||||
|
|
@ -605,9 +579,8 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
*hagl_min = rangefinder_hagl_min;
|
||||
*hagl_max = rangefinder_hagl_max;
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// Keep within flow AND range sensor limits when exclusively using optical flow
|
||||
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
|
||||
|
||||
|
|
@ -625,7 +598,9 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
*hagl_min = flow_hagl_min;
|
||||
*hagl_max = flow_hagl_max;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
}
|
||||
|
||||
void Ekf::resetImuBias()
|
||||
|
|
@ -767,11 +742,23 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
|||
tas = sqrtf(_aid_src_airspeed.test_ratio);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
hagl = NAN;
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_hagl_sensor_status.flags.range_finder) {
|
||||
// return the terrain height innovation test ratio
|
||||
hagl = sqrtf(_hagl_test_ratio);
|
||||
hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio);
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
// return the terrain height innovation test ratio
|
||||
hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]));
|
||||
}
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
// return the synthetic sideslip innovation test ratio
|
||||
beta = sqrtf(_aid_src_sideslip.test_ratio);
|
||||
|
|
@ -789,7 +776,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
|
|||
soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
|
||||
soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
|
||||
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
|
||||
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
|
||||
|
|
@ -927,12 +916,15 @@ float Ekf::getYawVar() const
|
|||
void Ekf::updateGroundEffect()
|
||||
{
|
||||
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
if (isTerrainEstimateValid()) {
|
||||
// automatically set ground effect if terrain is valid
|
||||
float height = _terrain_vpos - _state.pos(2);
|
||||
_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
|
||||
|
||||
} else if (_control_status.flags.gnd_effect) {
|
||||
} else
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
if (_control_status.flags.gnd_effect) {
|
||||
// Turn off ground effect compensation if it times out
|
||||
if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
|
||||
_control_status.flags.gnd_effect = false;
|
||||
|
|
|
|||
|
|
@ -123,17 +123,17 @@ void Ekf::controlMagFusion()
|
|||
|
||||
bool Ekf::checkHaglYawResetReq() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// We need to reset the yaw angle after climbing away from the ground to enable
|
||||
// recovery from ground level magnetic interference.
|
||||
if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
|
||||
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
||||
// and request a yaw reset if not already requested.
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
static constexpr float mag_anomalies_max_hagl = 1.5f;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
return above_mag_anomalies;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
}
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -209,7 +209,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
|
|||
if (_time_delayed_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
|
||||
// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
|
||||
// but use a relaxed time criteria to enable it to coast through bad range finder data
|
||||
if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
|
||||
if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6)) {
|
||||
fuseOptFlow();
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -204,6 +204,7 @@ void Ekf::controlRangeHeightFusion()
|
|||
|
||||
bool Ekf::isConditionalRangeAidSuitable()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
if (_control_status.flags.in_air
|
||||
&& _range_sensor.isHealthy()
|
||||
&& isTerrainEstimateValid()) {
|
||||
|
|
@ -212,7 +213,10 @@ bool Ekf::isConditionalRangeAidSuitable()
|
|||
float range_hagl_max = _params.max_hagl_for_range_aid;
|
||||
float max_vel_xy = _params.max_vel_for_range_aid;
|
||||
|
||||
const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var));
|
||||
const float hagl_innov = _aid_src_terrain_range_finder.innovation;
|
||||
const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance;
|
||||
|
||||
const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var));
|
||||
|
||||
bool is_hagl_stable = (hagl_test_ratio < 1.f);
|
||||
|
||||
|
|
@ -234,6 +238,7 @@ bool Ekf::isConditionalRangeAidSuitable()
|
|||
|
||||
return is_in_range && is_hagl_stable && is_below_max_speed;
|
||||
}
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -49,7 +49,9 @@ void Ekf::initHagl()
|
|||
stopHaglFlowFusion();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
stopHaglRngFusion();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
// assume a ground clearance
|
||||
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
|
||||
|
|
@ -68,10 +70,14 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
|
|||
|
||||
predictHagl(imu_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
controlHaglRngFusion();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
controlHaglFlowFusion();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
controlHaglFakeFusion();
|
||||
|
||||
// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
|
||||
|
|
@ -95,6 +101,7 @@ void Ekf::predictHagl(const imuSample &imu_delayed)
|
|||
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void Ekf::controlHaglRngFusion()
|
||||
{
|
||||
if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
|
||||
|
|
@ -104,20 +111,29 @@ void Ekf::controlHaglRngFusion()
|
|||
return;
|
||||
}
|
||||
|
||||
if (_range_sensor.isDataReady()) {
|
||||
updateHaglRng(_aid_src_terrain_range_finder);
|
||||
}
|
||||
|
||||
if (_range_sensor.isDataHealthy()) {
|
||||
const bool continuing_conditions_passing = _control_status.flags.in_air && _rng_consistency_check.isKinematicallyConsistent();
|
||||
//const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height
|
||||
const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f);
|
||||
|
||||
const bool continuing_conditions_passing = _control_status.flags.in_air
|
||||
//&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height
|
||||
&& _rng_consistency_check.isKinematicallyConsistent();
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& (_rng_consistency_check.getTestRatio() < 1.f);
|
||||
|
||||
_time_last_healthy_rng_data = _time_delayed_us;
|
||||
|
||||
if (_hagl_sensor_status.flags.range_finder) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseHaglRng();
|
||||
fuseHaglRng(_aid_src_terrain_range_finder);
|
||||
|
||||
// We have been rejecting range data for too long
|
||||
const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
|
||||
const bool is_fusion_failing = isTimedOut(_time_last_hagl_fuse, timeout);
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_terrain_range_finder.time_last_fuse, timeout);
|
||||
|
||||
if (is_fusion_failing) {
|
||||
if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) {
|
||||
|
|
@ -143,7 +159,16 @@ void Ekf::controlHaglRngFusion()
|
|||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startHaglRngFusion();
|
||||
_hagl_sensor_status.flags.range_finder = true;
|
||||
|
||||
// Reset the state to the measurement only if the test ratio is large,
|
||||
// otherwise let it converge through the fusion
|
||||
if (_hagl_sensor_status.flags.flow && (_aid_src_terrain_range_finder.test_ratio < 0.2f)) {
|
||||
fuseHaglRng(_aid_src_terrain_range_finder);
|
||||
|
||||
} else {
|
||||
resetHaglRng();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -151,42 +176,11 @@ void Ekf::controlHaglRngFusion()
|
|||
// No data anymore. Stop until it comes back.
|
||||
stopHaglRngFusion();
|
||||
}
|
||||
|
||||
_aid_src_terrain_range_finder.fusion_enabled = _hagl_sensor_status.flags.range_finder;
|
||||
}
|
||||
|
||||
void Ekf::startHaglRngFusion()
|
||||
{
|
||||
_hagl_sensor_status.flags.range_finder = true;
|
||||
resetHaglRngIfNeeded();
|
||||
}
|
||||
|
||||
void Ekf::resetHaglRngIfNeeded()
|
||||
{
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
const float meas_hagl = _range_sensor.getDistBottom();
|
||||
const float pred_hagl = _terrain_vpos - _state.pos(2);
|
||||
const float hagl_innov = pred_hagl - meas_hagl;
|
||||
const float obs_variance = getRngVar();
|
||||
|
||||
const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
|
||||
|
||||
const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
|
||||
const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var);
|
||||
|
||||
// Reset the state to the measurement only if the test ratio is large,
|
||||
// otherwise let it converge through the fusion
|
||||
if (hagl_test_ratio > 0.2f) {
|
||||
resetHaglRng();
|
||||
|
||||
} else {
|
||||
fuseHaglRng();
|
||||
}
|
||||
|
||||
} else {
|
||||
resetHaglRng();
|
||||
}
|
||||
}
|
||||
|
||||
float Ekf::getRngVar()
|
||||
float Ekf::getRngVar() const
|
||||
{
|
||||
return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f)
|
||||
+ sq(_params.range_noise)
|
||||
|
|
@ -198,26 +192,25 @@ void Ekf::resetHaglRng()
|
|||
_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
|
||||
_terrain_var = getRngVar();
|
||||
_terrain_vpos_reset_counter++;
|
||||
_time_last_hagl_fuse = _time_delayed_us;
|
||||
_time_last_healthy_rng_data = 0;
|
||||
|
||||
_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::stopHaglRngFusion()
|
||||
{
|
||||
if (_hagl_sensor_status.flags.range_finder) {
|
||||
ECL_INFO("stopping HAGL range fusion");
|
||||
|
||||
// reset flags
|
||||
resetEstimatorAidStatus(_aid_src_terrain_range_finder);
|
||||
|
||||
_hagl_innov = 0.f;
|
||||
_hagl_innov_var = 0.f;
|
||||
_hagl_test_ratio = 0.f;
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
|
||||
_hagl_sensor_status.flags.range_finder = false;
|
||||
}
|
||||
|
||||
_time_last_healthy_rng_data = 0;
|
||||
}
|
||||
|
||||
void Ekf::fuseHaglRng()
|
||||
void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
|
||||
{
|
||||
// get a height above ground measurement from the range finder assuming a flat earth
|
||||
const float meas_hagl = _range_sensor.getDistBottom();
|
||||
|
|
@ -226,33 +219,56 @@ void Ekf::fuseHaglRng()
|
|||
const float pred_hagl = _terrain_vpos - _state.pos(2);
|
||||
|
||||
// calculate the innovation
|
||||
_hagl_innov = pred_hagl - meas_hagl;
|
||||
const float hagl_innov = pred_hagl - meas_hagl;
|
||||
|
||||
// calculate the observation variance adding the variance of the vehicles own height uncertainty
|
||||
const float obs_variance = getRngVar();
|
||||
|
||||
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
|
||||
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
|
||||
const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
|
||||
|
||||
// perform an innovation consistency check and only fuse data if it passes
|
||||
const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
|
||||
_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
|
||||
const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f);
|
||||
|
||||
if (_hagl_test_ratio <= 1.0f) {
|
||||
|
||||
aid_src.timestamp_sample = _time_delayed_us; // TODO
|
||||
|
||||
aid_src.observation = pred_hagl;
|
||||
aid_src.observation_variance = obs_variance;
|
||||
|
||||
aid_src.innovation = hagl_innov;
|
||||
aid_src.innovation_variance = hagl_innov_var;
|
||||
|
||||
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
|
||||
|
||||
aid_src.fusion_enabled = false;
|
||||
aid_src.fused = false;
|
||||
}
|
||||
|
||||
void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src)
|
||||
{
|
||||
if (!aid_src.innovation_rejected) {
|
||||
// calculate the Kalman gain
|
||||
const float gain = _terrain_var / _hagl_innov_var;
|
||||
const float gain = _terrain_var / aid_src.innovation_variance;
|
||||
|
||||
// correct the state
|
||||
_terrain_vpos -= gain * _hagl_innov;
|
||||
_terrain_vpos -= gain * aid_src.innovation;
|
||||
|
||||
// correct the variance
|
||||
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
|
||||
|
||||
// record last successful fusion event
|
||||
_time_last_hagl_fuse = _time_delayed_us;
|
||||
_innov_check_fail_status.flags.reject_hagl = false;
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
aid_src.fused = true;
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_hagl = true;
|
||||
aid_src.fused = false;
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void Ekf::controlHaglFlowFusion()
|
||||
|
|
@ -268,8 +284,8 @@ void Ekf::controlHaglFlowFusion()
|
|||
const bool continuing_conditions_passing = _control_status.flags.in_air
|
||||
&& !_control_status.flags.opt_flow
|
||||
&& _control_status.flags.gps
|
||||
&& isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead?
|
||||
/* && !_hagl_sensor_status.flags.range_finder; */
|
||||
&& !_hagl_sensor_status.flags.range_finder;
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing;
|
||||
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
|
|
@ -277,10 +293,9 @@ void Ekf::controlHaglFlowFusion()
|
|||
|
||||
// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
|
||||
fuseFlowForTerrain(_aid_src_terrain_optical_flow);
|
||||
_flow_data_ready = false;
|
||||
|
||||
// TODO: do something when failing continuously the innovation check
|
||||
/* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */
|
||||
/* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */
|
||||
|
||||
/* if (is_fusion_failing) { */
|
||||
/* resetHaglFlow(); */
|
||||
|
|
@ -292,28 +307,27 @@ void Ekf::controlHaglFlowFusion()
|
|||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startHaglFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_hagl_sensor_status.flags.flow
|
||||
&& (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
stopHaglFlowFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::startHaglFlowFusion()
|
||||
{
|
||||
_hagl_sensor_status.flags.flow = true;
|
||||
// TODO: do a reset instead of trying to fuse the data?
|
||||
fuseFlowForTerrain(_aid_src_terrain_optical_flow);
|
||||
}
|
||||
}
|
||||
|
||||
_flow_data_ready = false;
|
||||
|
||||
} else if (_hagl_sensor_status.flags.flow && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
stopHaglFlowFusion();
|
||||
}
|
||||
|
||||
_aid_src_terrain_optical_flow.fusion_enabled = _hagl_sensor_status.flags.flow;
|
||||
}
|
||||
|
||||
void Ekf::stopHaglFlowFusion()
|
||||
{
|
||||
if (_hagl_sensor_status.flags.flow) {
|
||||
ECL_INFO("stopping HAGL flow fusion");
|
||||
|
||||
_hagl_sensor_status.flags.flow = false;
|
||||
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
|
||||
}
|
||||
|
|
@ -325,11 +339,14 @@ void Ekf::resetHaglFlow()
|
|||
_terrain_vpos = fmaxf(0.0f, _state.pos(2));
|
||||
_terrain_var = 100.0f;
|
||||
_terrain_vpos_reset_counter++;
|
||||
|
||||
_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
|
||||
{
|
||||
flow.fusion_enabled = true;
|
||||
flow.fused = true;
|
||||
|
||||
const float R_LOS = flow.observation_variance[0];
|
||||
|
||||
|
|
@ -397,9 +414,8 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
|
|||
_fault_status.flags.bad_optflow_X = false;
|
||||
_fault_status.flags.bad_optflow_Y = false;
|
||||
|
||||
_time_last_flow_terrain_fuse = _time_delayed_us;
|
||||
//_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain?
|
||||
_aid_src_optical_flow.fused = true;
|
||||
flow.time_last_fuse = _time_delayed_us;
|
||||
flow.fused = true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
|
|
@ -416,18 +432,22 @@ void Ekf::controlHaglFakeFusion()
|
|||
bool Ekf::isTerrainEstimateValid() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
// we have been fusing range finder measurements in the last 5 seconds
|
||||
if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
|
||||
if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
|
||||
// this can only be the case if the main filter does not fuse optical flow
|
||||
if (_hagl_sensor_status.flags.flow && isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6)) {
|
||||
if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -120,13 +120,20 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_baro_ctrl(_params->baro_ctrl),
|
||||
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
|
||||
_param_ekf2_noaid_tout(_params->valid_timeout_max),
|
||||
#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_param_ekf2_min_rng(_params->rng_gnd_clearance),
|
||||
#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
_param_ekf2_terr_mask(_params->terrain_fusion_mode),
|
||||
_param_ekf2_terr_noise(_params->terrain_p_noise),
|
||||
_param_ekf2_terr_grad(_params->terrain_gradient),
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_param_ekf2_rng_ctrl(_params->rng_ctrl),
|
||||
_param_ekf2_rng_delay(_params->range_delay_ms),
|
||||
_param_ekf2_rng_noise(_params->range_noise),
|
||||
_param_ekf2_rng_sfe(_params->range_noise_scaler),
|
||||
_param_ekf2_rng_gate(_params->range_innov_gate),
|
||||
_param_ekf2_min_rng(_params->rng_gnd_clearance),
|
||||
_param_ekf2_rng_pitch(_params->rng_sens_pitch),
|
||||
_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
|
||||
_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
|
||||
|
|
@ -136,9 +143,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
|
||||
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
|
||||
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
|
||||
_param_ekf2_terr_mask(_params->terrain_fusion_mode),
|
||||
_param_ekf2_terr_noise(_params->terrain_p_noise),
|
||||
_param_ekf2_terr_grad(_params->terrain_gradient),
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_param_ekf2_ev_delay(_params->ev_delay_ms),
|
||||
|
|
@ -1022,9 +1026,22 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// optical flow
|
||||
PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range finder
|
||||
PublishAidSourceStatus(_ekf.aid_src_terrain_range_finder(), _status_terrain_range_finder_pub_last,
|
||||
_estimator_aid_src_terrain_range_finder_pub);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// optical flow
|
||||
PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last,
|
||||
_estimator_aid_src_terrain_optical_flow_pub);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
}
|
||||
|
||||
void EKF2::PublishAttitude(const hrt_abstime ×tamp)
|
||||
|
|
@ -1247,7 +1264,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
|||
global_pos.terrain_alt = NAN;
|
||||
global_pos.terrain_alt_valid = false;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
|
||||
if (_ekf.isTerrainEstimateValid()) {
|
||||
// Terrain altitude in m, WGS84
|
||||
|
|
@ -1255,7 +1272,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
|||
global_pos.terrain_alt_valid = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
|
||||
|| _ekf.control_status_flags().wind_dead_reckoning;
|
||||
|
|
@ -1318,7 +1335,6 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnov(innovations.flow);
|
||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnov(innovations.heading);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
|
@ -1333,11 +1349,18 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
_ekf.getBetaInnov(innovations.beta);
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnov(innovations.hagl);
|
||||
_ekf.getHaglRateInnov(innovations.hagl_rate);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_ekf.getGravityInnov(innovations.gravity);
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnov(innovations.hagl);
|
||||
# endif //CONFIG_EKF2_RANGE_FINDER
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// Not yet supported
|
||||
innovations.aux_vvel = NAN;
|
||||
|
||||
|
|
@ -1355,11 +1378,13 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// set dist bottom to scale flow innovation
|
||||
const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
|
||||
_preflt_checker.setDistBottom(dist_bottom);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
|
||||
|
|
@ -1392,13 +1417,13 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
|||
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
|
||||
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnovRatio(test_ratios.flow[0]);
|
||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovRatio(test_ratios.heading);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
|
@ -1413,11 +1438,18 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
_ekf.getBetaInnovRatio(test_ratios.beta);
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnovRatio(test_ratios.hagl);
|
||||
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnovRatio(test_ratios.hagl);
|
||||
# endif
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// Not yet supported
|
||||
test_ratios.aux_vvel = NAN;
|
||||
|
||||
|
|
@ -1437,13 +1469,13 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
_ekf.getBaroHgtInnovVar(variances.baro_vpos);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovVar(variances.rng_vpos);
|
||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovVar(variances.aux_hvel);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnovVar(variances.flow);
|
||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovVar(variances.heading);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
|
@ -1458,11 +1490,18 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
_ekf.getBetaInnovVar(variances.beta);
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnovVar(variances.hagl);
|
||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_ekf.getGravityInnovVar(variances.gravity);
|
||||
|
||||
// Not yet supported
|
||||
variances.aux_vvel = NAN;
|
||||
|
||||
|
|
@ -1530,10 +1569,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp)
|
|||
lpos.delta_heading = Eulerf(delta_q_reset).psi();
|
||||
lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// Distance to bottom surface (ground) in meters, must be positive
|
||||
lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f);
|
||||
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
|
||||
lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
|
||||
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
|
||||
|
|
|
|||
|
|
@ -303,7 +303,6 @@ private:
|
|||
hrt_abstime _last_gps_status_published{0};
|
||||
|
||||
hrt_abstime _status_baro_hgt_pub_last{0};
|
||||
hrt_abstime _status_rng_hgt_pub_last{0};
|
||||
|
||||
hrt_abstime _status_fake_hgt_pub_last{0};
|
||||
hrt_abstime _status_fake_pos_pub_last{0};
|
||||
|
|
@ -353,7 +352,6 @@ private:
|
|||
hrt_abstime _status_gnss_yaw_pub_last {0};
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
|
||||
hrt_abstime _status_gravity_pub_last{0};
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
|
|
@ -365,20 +363,31 @@ private:
|
|||
perf_counter_t _msg_missed_landing_target_pose_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_terrain_range_finder_pub {ORB_ID(estimator_aid_src_terrain_range_finder)};
|
||||
hrt_abstime _status_terrain_range_finder_pub_last{0};
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub {ORB_ID(estimator_aid_src_terrain_optical_flow)};
|
||||
hrt_abstime _status_terrain_optical_flow_pub_last{0};
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)};
|
||||
uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
|
||||
hrt_abstime _status_optical_flow_pub_last{0};
|
||||
hrt_abstime _status_terrain_optical_flow_pub_last{0};
|
||||
|
||||
perf_counter_t _msg_missed_optical_flow_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
float _last_baro_bias_published{};
|
||||
float _last_gnss_hgt_bias_published{};
|
||||
float _last_rng_hgt_bias_published{};
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
uORB::Subscription _airspeed_sub {ORB_ID(airspeed)};
|
||||
|
|
@ -414,6 +423,9 @@ private:
|
|||
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
hrt_abstime _status_rng_hgt_pub_last {0};
|
||||
float _last_rng_hgt_bias_published{};
|
||||
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)};
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
|
||||
|
||||
|
|
@ -590,42 +602,30 @@ private:
|
|||
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
|
||||
_param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng,
|
||||
#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
(ParamExtInt<px4::params::EKF2_TERR_MASK>) _param_ekf2_terr_mask,
|
||||
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>) _param_ekf2_terr_grad,
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range finder fusion
|
||||
(ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl, ///< range finder control selection
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
|
||||
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>)
|
||||
_param_ekf2_rng_noise, ///< observation noise for range finder measurements (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_SFE>)
|
||||
_param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_GATE>)
|
||||
_param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_MIN_RNG>)
|
||||
_param_ekf2_min_rng, ///< minimum valid value for range when on ground (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>)
|
||||
_param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>)
|
||||
_param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>)
|
||||
_param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>)
|
||||
_param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>)
|
||||
_param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>)
|
||||
_param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>)
|
||||
_param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>)
|
||||
_param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m)
|
||||
|
||||
(ParamExtInt<px4::params::EKF2_TERR_MASK>)
|
||||
_param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation
|
||||
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec)
|
||||
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
|
||||
_param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m)
|
||||
(ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>) _param_ekf2_rng_delay,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _param_ekf2_rng_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_GATE>) _param_ekf2_rng_gate,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>) _param_ekf2_rng_a_igate,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z,
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ menuconfig EKF2_OPTICAL_FLOW
|
|||
depends on MODULES_EKF2
|
||||
bool "optical flow fusion support"
|
||||
default y
|
||||
select EKF2_TERRAIN
|
||||
depends on EKF2_RANGE_FINDER
|
||||
---help---
|
||||
EKF2 optical flow fusion support.
|
||||
|
|
@ -85,6 +86,14 @@ depends on MODULES_EKF2
|
|||
---help---
|
||||
EKF2 sideslip fusion support.
|
||||
|
||||
menuconfig EKF2_TERRAIN
|
||||
depends on MODULES_EKF2
|
||||
bool "terrain estimator support"
|
||||
default y
|
||||
depends on EKF2_OPTICAL_FLOW || EKF2_RANGE_FINDER
|
||||
---help---
|
||||
EKF2 terrain estimator support.
|
||||
|
||||
menuconfig USER_EKF2
|
||||
bool "ekf2 running as userspace module"
|
||||
default n
|
||||
|
|
|
|||
Loading…
Reference in New Issue