forked from Archive/PX4-Autopilot
ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)
This commit is contained in:
Daniel Agar
parent
0784901a66
commit
32a5bd32ad
|
|
@ -87,10 +87,6 @@ list(APPEND EKF_SRCS
|
|||
EKF/mag_control.cpp
|
||||
EKF/mag_fusion.cpp
|
||||
EKF/output_predictor.cpp
|
||||
EKF/range_finder_consistency_check.cpp
|
||||
EKF/range_height_control.cpp
|
||||
EKF/sensor_range_finder.cpp
|
||||
EKF/terrain_estimator.cpp
|
||||
EKF/vel_pos_fusion.cpp
|
||||
EKF/zero_innovation_heading_update.cpp
|
||||
EKF/zero_velocity_update.cpp
|
||||
|
|
@ -129,6 +125,15 @@ if(CONFIG_EKF2_OPTICAL_FLOW)
|
|||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_RANGE_FINDER)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/range_finder_consistency_check.cpp
|
||||
EKF/range_height_control.cpp
|
||||
EKF/sensor_range_finder.cpp
|
||||
EKF/terrain_estimator.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_SIDESLIP)
|
||||
list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -52,10 +52,6 @@ list(APPEND EKF_SRCS
|
|||
mag_control.cpp
|
||||
mag_fusion.cpp
|
||||
output_predictor.cpp
|
||||
range_finder_consistency_check.cpp
|
||||
range_height_control.cpp
|
||||
sensor_range_finder.cpp
|
||||
terrain_estimator.cpp
|
||||
vel_pos_fusion.cpp
|
||||
zero_innovation_heading_update.cpp
|
||||
zero_velocity_update.cpp
|
||||
|
|
@ -94,6 +90,15 @@ if(CONFIG_EKF2_OPTICAL_FLOW)
|
|||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_RANGE_FINDER)
|
||||
list(APPEND EKF_SRCS
|
||||
range_finder_consistency_check.cpp
|
||||
range_height_control.cpp
|
||||
sensor_range_finder.cpp
|
||||
terrain_estimator.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_SIDESLIP)
|
||||
list(APPEND EKF_SRCS sideslip_fusion.cpp)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -69,16 +69,17 @@ void Ekf::initialiseCovariance()
|
|||
// position
|
||||
P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));
|
||||
P(8,8) = P(7,7);
|
||||
P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
|
||||
|
||||
} else if (_control_status.flags.gps_hgt) {
|
||||
P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
|
||||
|
||||
} else {
|
||||
P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
// gyro bias
|
||||
_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
|
||||
|
|
@ -453,7 +454,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
|
|||
if (bad_vz_gps || bad_vz_ev) {
|
||||
bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
|
||||
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);
|
||||
#else
|
||||
bool bad_z_rng = false;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f);
|
||||
|
|
|
|||
|
|
@ -63,9 +63,11 @@ void Ekf::reset()
|
|||
_state.wind_vel.setZero();
|
||||
_state.quat_nominal.setIdentity();
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
|
||||
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
|
||||
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_control_status.value = 0;
|
||||
_control_status_prev.value = 0;
|
||||
|
|
@ -94,17 +96,13 @@ void Ekf::reset()
|
|||
_time_last_hor_vel_fuse = 0;
|
||||
_time_last_ver_vel_fuse = 0;
|
||||
_time_last_heading_fuse = 0;
|
||||
|
||||
_time_last_flow_terrain_fuse = 0;
|
||||
_time_last_zero_velocity_fuse = 0;
|
||||
_time_last_healthy_rng_data = 0;
|
||||
|
||||
_last_known_pos.setZero();
|
||||
|
||||
_time_acc_bias_check = 0;
|
||||
|
||||
_gps_checks_passed = false;
|
||||
|
||||
_gps_alt_ref = NAN;
|
||||
|
||||
_baro_counter = 0;
|
||||
|
|
@ -115,7 +113,6 @@ void Ekf::reset()
|
|||
_clip_counter = 0;
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_baro_hgt);
|
||||
resetEstimatorAidStatus(_aid_src_rng_hgt);
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
resetEstimatorAidStatus(_aid_src_airspeed);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
|
@ -152,6 +149,10 @@ void Ekf::reset()
|
|||
resetEstimatorAidStatus(_aid_src_optical_flow);
|
||||
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
resetEstimatorAidStatus(_aid_src_rng_hgt);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
}
|
||||
|
||||
bool Ekf::update()
|
||||
|
|
@ -179,8 +180,10 @@ bool Ekf::update()
|
|||
// control fusion of observation data
|
||||
controlFusionModes(imu_sample_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// run a separate filter for terrain estimation
|
||||
runTerrainEstimator(imu_sample_delayed);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
|
||||
_state.quat_nominal, _state.vel, _state.pos);
|
||||
|
|
@ -218,8 +221,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)
|
||||
// Initialise the terrain estimator
|
||||
initHagl();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
// reset the output predictor state history to match the EKF initial values
|
||||
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
|
||||
|
|
|
|||
|
|
@ -95,17 +95,41 @@ 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; }
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
void getAuxVelInnov(float aux_vel_innov[2]) const;
|
||||
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
|
||||
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
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(); }
|
||||
|
||||
// terrain estimate
|
||||
bool isTerrainEstimateValid() const;
|
||||
|
||||
uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
float getTerrainVertPos() const { return _terrain_vpos; };
|
||||
|
||||
// get the number of times the vertical terrain position has been reset
|
||||
uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
|
||||
|
||||
// get the terrain variance
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
#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 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]); }
|
||||
|
|
@ -123,10 +147,15 @@ public:
|
|||
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_optical_flow() const { return _aid_src_optical_flow; }
|
||||
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
void getAuxVelInnov(float aux_vel_innov[2]) const;
|
||||
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
|
||||
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
void getHeadingInnov(float &heading_innov) const
|
||||
{
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
|
|
@ -230,14 +259,6 @@ public:
|
|||
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
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(); }
|
||||
|
||||
void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
|
||||
void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
|
||||
void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
|
||||
|
|
@ -331,8 +352,6 @@ public:
|
|||
return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
|
||||
}
|
||||
|
||||
bool isTerrainEstimateValid() const;
|
||||
|
||||
bool isYawFinalAlignComplete() const
|
||||
{
|
||||
const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
|
||||
|
|
@ -343,17 +362,6 @@ public:
|
|||
&& (is_mag_alignment_in_flight_complete || !is_using_mag);
|
||||
}
|
||||
|
||||
uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
float getTerrainVertPos() const { return _terrain_vpos; };
|
||||
|
||||
// get the number of times the vertical terrain position has been reset
|
||||
uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
|
||||
|
||||
// get the terrain variance
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
|
||||
// gyro bias (states 10, 11, 12)
|
||||
Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
|
||||
Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
|
||||
|
|
@ -454,7 +462,7 @@ public:
|
|||
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
|
||||
const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
|
||||
const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
|
||||
const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
|
||||
|
||||
|
|
@ -470,7 +478,6 @@ public:
|
|||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
|
||||
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
|
||||
|
||||
const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
|
||||
const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
|
||||
|
|
@ -540,8 +547,6 @@ private:
|
|||
|
||||
// booleans true when fresh sensor data is available at the fusion time horizon
|
||||
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
|
||||
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
|
||||
bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
|
||||
|
||||
uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
|
||||
uint64_t _time_last_v_pos_aiding{0};
|
||||
|
|
@ -552,10 +557,7 @@ private:
|
|||
uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec)
|
||||
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
|
||||
uint64_t _time_last_heading_fuse{0};
|
||||
|
||||
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
|
||||
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
|
||||
uint64_t _time_last_healthy_rng_data{0};
|
||||
|
||||
Vector3f _last_known_pos{}; ///< last known local position vector (m)
|
||||
|
||||
|
|
@ -589,8 +591,30 @@ 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};
|
||||
|
||||
// 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
|
||||
|
||||
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
|
||||
#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)
|
||||
|
|
@ -603,8 +627,11 @@ private:
|
|||
uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec)
|
||||
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{};
|
||||
estimator_aid_source1d_s _aid_src_rng_hgt{};
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
estimator_aid_source1d_s _aid_src_airspeed{};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
|
@ -647,11 +674,6 @@ private:
|
|||
estimator_aid_source2d_s _aid_src_aux_vel{};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
estimator_aid_source2d_s _aid_src_optical_flow{};
|
||||
estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec)
|
||||
Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec)
|
||||
|
|
@ -678,7 +700,6 @@ private:
|
|||
AlphaFilter<float> _baro_lpf{0.1f}; ///< filtered barometric height measurement (m)
|
||||
|
||||
// Variables used to control activation of post takeoff functionality
|
||||
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
|
||||
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
|
||||
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
|
||||
Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
|
||||
|
|
@ -696,13 +717,6 @@ private:
|
|||
Vector3f _prev_delta_ang_bias_var{}; ///< saved delta angle XYZ bias variances (rad/sec)
|
||||
Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2
|
||||
|
||||
// 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
|
||||
|
||||
// height sensor status
|
||||
bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags
|
||||
bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent
|
||||
|
|
@ -829,16 +843,19 @@ private:
|
|||
void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
|
||||
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
|
||||
|
||||
// calculate optical flow body angular rate compensation
|
||||
// returns false if bias corrected body rate data is unavailable
|
||||
bool calcOptFlowBodyRateComp();
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// range height
|
||||
void controlRangeHeightFusion();
|
||||
bool isConditionalRangeAidSuitable();
|
||||
void stopRngHgtFusion();
|
||||
|
||||
// initialise the terrain vertical position estimator
|
||||
// terrain vertical position estimator
|
||||
void initHagl();
|
||||
|
||||
void runTerrainEstimator(const imuSample &imu_delayed);
|
||||
void predictHagl(const imuSample &imu_delayed);
|
||||
|
||||
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
|
||||
|
||||
// update the terrain vertical position estimate using a height above ground measurement from the range finder
|
||||
void controlHaglRngFusion();
|
||||
void fuseHaglRng();
|
||||
|
|
@ -848,22 +865,37 @@ private:
|
|||
void stopHaglRngFusion();
|
||||
float getRngVar();
|
||||
|
||||
void controlHaglFakeFusion();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// 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);
|
||||
// control fusion of optical flow observations
|
||||
void controlOpticalFlowFusion(const imuSample &imu_delayed);
|
||||
void stopFlowFusion();
|
||||
|
||||
void updateOnGroundMotionForOpticalFlowChecks();
|
||||
void resetOnGroundMotionForOpticalFlowChecks();
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor
|
||||
float calcOptFlowMeasVar(const flowSample &flow_sample);
|
||||
|
||||
// calculate optical flow body angular rate compensation
|
||||
// returns false if bias corrected body rate data is unavailable
|
||||
bool calcOptFlowBodyRateComp();
|
||||
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void updateOptFlow(estimator_aid_source2d_s &aid_src);
|
||||
void fuseOptFlow();
|
||||
float predictFlowRange();
|
||||
Vector2f predictFlowVelBody();
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
void controlHaglFakeFusion();
|
||||
// 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
|
||||
|
||||
// reset the heading and magnetic field states using the declination and magnetometer measurements
|
||||
// return true if successful
|
||||
|
|
@ -981,11 +1013,6 @@ private:
|
|||
void stopEvYawFusion();
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// control fusion of optical flow observations
|
||||
void controlOpticalFlowFusion(const imuSample &imu_delayed);
|
||||
void updateOnGroundMotionForOpticalFlowChecks();
|
||||
void resetOnGroundMotionForOpticalFlowChecks();
|
||||
|
||||
// control fusion of GPS observations
|
||||
void controlGpsFusion(const imuSample &imu_delayed);
|
||||
bool shouldResetGpsFusion() const;
|
||||
|
|
@ -994,8 +1021,6 @@ private:
|
|||
// control fusion of magnetometer observations
|
||||
void controlMagFusion();
|
||||
|
||||
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
|
||||
|
||||
bool magReset();
|
||||
bool haglYawResetReq();
|
||||
|
||||
|
|
@ -1038,9 +1063,6 @@ private:
|
|||
void checkHeightSensorRefFallback();
|
||||
void controlBaroHeightFusion();
|
||||
void controlGnssHeightFusion(const gpsSample &gps_sample);
|
||||
void controlRangeHeightFusion();
|
||||
|
||||
bool isConditionalRangeAidSuitable();
|
||||
|
||||
void stopMagFusion();
|
||||
void stopMag3DFusion();
|
||||
|
|
@ -1050,16 +1072,12 @@ private:
|
|||
|
||||
void stopBaroHgtFusion();
|
||||
void stopGpsHgtFusion();
|
||||
void stopRngHgtFusion();
|
||||
|
||||
void updateGroundEffect();
|
||||
|
||||
// gravity fusion: heuristically enable / disable gravity fusion
|
||||
void controlGravityFusion(const imuSample &imu_delayed);
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor
|
||||
float calcOptFlowMeasVar(const flowSample &flow_sample);
|
||||
|
||||
// initialise the quaternion covariances using rotation vector variances
|
||||
// do not call before quaternion states are initialised
|
||||
void initialiseQuatCovariances(Vector3f &rot_vec_var);
|
||||
|
|
@ -1075,9 +1093,6 @@ private:
|
|||
void resetWind();
|
||||
void resetWindToZero();
|
||||
|
||||
// check that the range finder data is continuous
|
||||
void updateRangeDataContinuity();
|
||||
|
||||
// Increase the yaw error variance of the quaternions
|
||||
// Argument is additional yaw variance in rad**2
|
||||
void increaseQuatYawErrVariance(float yaw_variance);
|
||||
|
|
@ -1116,8 +1131,6 @@ private:
|
|||
void stopGpsPosFusion();
|
||||
void stopGpsVelFusion();
|
||||
|
||||
void stopFlowFusion();
|
||||
|
||||
void resetFakePosFusion();
|
||||
void stopFakePosFusion();
|
||||
|
||||
|
|
@ -1138,7 +1151,6 @@ private:
|
|||
|
||||
HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
|
||||
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
|
||||
HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
|
||||
|
|
|
|||
|
|
@ -202,7 +202,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
|
|||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
|
|
@ -564,6 +566,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
*hagl_min = NAN;
|
||||
*hagl_max = NAN;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// Calculate range finder limits
|
||||
const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
|
||||
|
||||
|
|
@ -578,6 +581,7 @@ 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)
|
||||
// Keep within flow AND range sensor limits when exclusively using optical flow
|
||||
|
|
@ -708,10 +712,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
|||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio);
|
||||
n_hgt_sources++;
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
|
|
@ -732,8 +738,10 @@ 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)
|
||||
// return the terrain height innovation test ratio
|
||||
hagl = sqrtf(_hagl_test_ratio);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
// return the synthetic sideslip innovation test ratio
|
||||
|
|
@ -744,7 +752,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
|||
// return a bitmask integer that describes which state estimates are valid
|
||||
void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
{
|
||||
ekf_solution_status_u soln_status;
|
||||
ekf_solution_status_u soln_status{};
|
||||
// TODO: Is this accurate enough?
|
||||
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
|
||||
soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
|
||||
|
|
@ -752,7 +760,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_RANGE_FINDER)
|
||||
soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
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;
|
||||
|
|
@ -1016,12 +1026,15 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
|
|||
void Ekf::updateGroundEffect()
|
||||
{
|
||||
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
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_RANGE_FINDER
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -49,13 +49,15 @@ EstimatorInterface::~EstimatorInterface()
|
|||
delete _gps_buffer;
|
||||
delete _mag_buffer;
|
||||
delete _baro_buffer;
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
delete _range_buffer;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
delete _airspeed_buffer;
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
delete _flow_buffer;
|
||||
#endif // _flow_buffer
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
delete _ext_vision_buffer;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
|
@ -288,6 +290,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
|
|||
}
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void EstimatorInterface::setRangeData(const rangeSample &range_sample)
|
||||
{
|
||||
if (!_initialised) {
|
||||
|
|
@ -323,6 +326,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
|
|||
ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
||||
|
|
@ -555,10 +559,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// using range finder
|
||||
if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
|
||||
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
if (_params.gnss_ctrl > 0) {
|
||||
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
|
||||
|
|
@ -700,9 +706,11 @@ void EstimatorInterface::print_status()
|
|||
printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_range_buffer) {
|
||||
printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
if (_airspeed_buffer) {
|
||||
|
|
|
|||
|
|
@ -64,11 +64,14 @@
|
|||
#include "common.h"
|
||||
#include "RingBuffer.h"
|
||||
#include "imu_down_sampler.hpp"
|
||||
#include "range_finder_consistency_check.hpp"
|
||||
#include "sensor_range_finder.hpp"
|
||||
#include "utils.hpp"
|
||||
#include "output_predictor.h"
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
# include "range_finder_consistency_check.hpp"
|
||||
# include "sensor_range_finder.hpp"
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
|
@ -94,8 +97,18 @@ public:
|
|||
void setAirspeedData(const airspeedSample &airspeed_sample);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void setRangeData(const rangeSample &range_sample);
|
||||
|
||||
// set sensor limitations reported by the rangefinder
|
||||
void set_rangefinder_limits(float min_distance, float max_distance)
|
||||
{
|
||||
_range_sensor.setLimits(min_distance, max_distance);
|
||||
}
|
||||
|
||||
const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
|
||||
void setOpticalFlowData(const flowSample &flow);
|
||||
|
|
@ -163,12 +176,6 @@ public:
|
|||
// set air density used by the multi-rotor specific drag force fusion
|
||||
void set_air_density(float air_density) { _air_density = air_density; }
|
||||
|
||||
// set sensor limitations reported by the rangefinder
|
||||
void set_rangefinder_limits(float min_distance, float max_distance)
|
||||
{
|
||||
_range_sensor.setLimits(min_distance, max_distance);
|
||||
}
|
||||
|
||||
// the flags considered are opt_flow, gps, ev_vel and ev_pos
|
||||
bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
|
||||
|
||||
|
|
@ -250,7 +257,6 @@ public:
|
|||
const uint64_t &time_delayed_us() const { return _time_delayed_us; }
|
||||
|
||||
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
|
||||
const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
|
||||
|
||||
const bool &global_origin_valid() const { return _NED_origin_initialised; }
|
||||
const MapProjection &global_origin() const { return _pos_ref; }
|
||||
|
|
@ -298,7 +304,7 @@ protected:
|
|||
|
||||
// measurement samples capturing measurements on the delayed time horizon
|
||||
gpsSample _gps_sample_delayed{};
|
||||
sensor::SensorRangeFinder _range_sensor{};
|
||||
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
airspeedSample _airspeed_sample_delayed{};
|
||||
|
|
@ -308,9 +314,13 @@ protected:
|
|||
extVisionSample _ev_sample_prev{};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
RangeFinderConsistencyCheck _rng_consistency_check;
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
RingBuffer<rangeSample> *_range_buffer{nullptr};
|
||||
uint64_t _time_last_range_buffer_push{0};
|
||||
|
||||
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
|
||||
sensor::SensorRangeFinder _range_sensor{};
|
||||
RangeFinderConsistencyCheck _rng_consistency_check;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
RingBuffer<flowSample> *_flow_buffer{nullptr};
|
||||
|
|
@ -323,6 +333,8 @@ protected:
|
|||
float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m)
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
|
||||
|
||||
bool _imu_updated{false}; // true if the ekf should update (completed downsampling process)
|
||||
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
|
||||
|
||||
|
|
@ -339,8 +351,6 @@ protected:
|
|||
uint64_t _time_last_gps_yaw_buffer_push{0};
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
|
||||
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
RingBuffer<dragSample> *_drag_buffer{nullptr};
|
||||
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
|
||||
|
|
@ -367,7 +377,7 @@ protected:
|
|||
RingBuffer<gpsSample> *_gps_buffer{nullptr};
|
||||
RingBuffer<magSample> *_mag_buffer{nullptr};
|
||||
RingBuffer<baroSample> *_baro_buffer{nullptr};
|
||||
RingBuffer<rangeSample> *_range_buffer{nullptr};
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
|
@ -384,7 +394,6 @@ protected:
|
|||
uint64_t _time_last_gps_buffer_push{0};
|
||||
uint64_t _time_last_mag_buffer_push{0};
|
||||
uint64_t _time_last_baro_buffer_push{0};
|
||||
uint64_t _time_last_range_buffer_push{0};
|
||||
|
||||
uint64_t _time_last_gnd_effect_on{0};
|
||||
|
||||
|
|
|
|||
|
|
@ -45,7 +45,9 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
|
|||
|
||||
controlBaroHeightFusion();
|
||||
controlGnssHeightFusion(_gps_sample_delayed);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
controlRangeHeightFusion();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
checkHeightSensorRefFallback();
|
||||
}
|
||||
|
|
@ -185,9 +187,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
|||
checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]};
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
|
|
|
|||
|
|
@ -238,9 +238,13 @@ bool Ekf::haglYawResetReq()
|
|||
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;
|
||||
#else
|
||||
return true;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
}
|
||||
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -199,6 +199,7 @@ void Ekf::resetHaglRng()
|
|||
_terrain_var = getRngVar();
|
||||
_terrain_vpos_reset_counter++;
|
||||
_time_last_hagl_fuse = _time_delayed_us;
|
||||
_time_last_healthy_rng_data = 0;
|
||||
}
|
||||
|
||||
void Ekf::stopHaglRngFusion()
|
||||
|
|
@ -212,6 +213,8 @@ void Ekf::stopHaglRngFusion()
|
|||
|
||||
_hagl_sensor_status.flags.range_finder = false;
|
||||
}
|
||||
|
||||
_time_last_healthy_rng_data = 0;
|
||||
}
|
||||
|
||||
void Ekf::fuseHaglRng()
|
||||
|
|
@ -417,11 +420,13 @@ bool Ekf::isTerrainEstimateValid() const
|
|||
return true;
|
||||
}
|
||||
|
||||
#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)) {
|
||||
return true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,7 +63,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_mag_delay(_params->mag_delay_ms),
|
||||
_param_ekf2_baro_delay(_params->baro_delay_ms),
|
||||
_param_ekf2_gps_delay(_params->gps_delay_ms),
|
||||
_param_ekf2_rng_delay(_params->range_delay_ms),
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
|
@ -74,8 +73,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_mag_e_noise(_params->mage_p_noise),
|
||||
_param_ekf2_mag_b_noise(_params->magb_p_noise),
|
||||
_param_ekf2_wind_nsd(_params->wind_vel_nsd),
|
||||
_param_ekf2_terr_noise(_params->terrain_p_noise),
|
||||
_param_ekf2_terr_grad(_params->terrain_gradient),
|
||||
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
|
||||
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
|
||||
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
|
||||
|
|
@ -117,9 +114,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_hgt_ref(_params->height_sensor_ref),
|
||||
_param_ekf2_baro_ctrl(_params->baro_ctrl),
|
||||
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
|
||||
_param_ekf2_rng_ctrl(_params->rng_ctrl),
|
||||
_param_ekf2_terr_mask(_params->terrain_fusion_mode),
|
||||
_param_ekf2_noaid_tout(_params->valid_timeout_max),
|
||||
#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),
|
||||
|
|
@ -130,6 +128,13 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
|
||||
_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
|
||||
_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
|
||||
_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),
|
||||
_param_ekf2_ev_ctrl(_params->ev_ctrl),
|
||||
|
|
@ -160,9 +165,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
|
||||
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
|
||||
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
|
||||
_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_gbias_init(_params->switch_on_gyro_bias),
|
||||
_param_ekf2_abias_init(_params->switch_on_accel_bias),
|
||||
_param_ekf2_angerr_init(_params->initial_tilt_err),
|
||||
|
|
@ -212,7 +214,9 @@ EKF2::~EKF2()
|
|||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
perf_free(_msg_missed_airspeed_perf);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
perf_free(_msg_missed_distance_sensor_perf);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
perf_free(_msg_missed_gps_perf);
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
perf_free(_msg_missed_landing_target_pose_perf);
|
||||
|
|
@ -294,12 +298,16 @@ bool EKF2::multi_init(int imu, int mag)
|
|||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
// RNG advertise
|
||||
if (_param_ekf2_rng_ctrl.get()) {
|
||||
_estimator_aid_src_rng_hgt_pub.advertise();
|
||||
_estimator_rng_hgt_bias_pub.advertise();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_attitude_pub.advertise();
|
||||
_local_position_pub.advertise();
|
||||
_global_position_pub.advertise();
|
||||
|
|
@ -341,7 +349,9 @@ int EKF2::print_status()
|
|||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
perf_print_counter(_msg_missed_airspeed_perf);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
perf_print_counter(_msg_missed_distance_sensor_perf);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
perf_print_counter(_msg_missed_gps_perf);
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
perf_print_counter(_msg_missed_landing_target_pose_perf);
|
||||
|
|
@ -658,7 +668,9 @@ void EKF2::Run()
|
|||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
UpdateGpsSample(ekf2_timestamps);
|
||||
UpdateMagSample(ekf2_timestamps);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
UpdateRangeSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
UpdateSystemFlagsSample(ekf2_timestamps);
|
||||
|
||||
// run the EKF update and output
|
||||
|
|
@ -675,7 +687,9 @@ void EKF2::Run()
|
|||
// publish status/logging messages
|
||||
PublishBaroBias(now);
|
||||
PublishGnssHgtBias(now);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
PublishRngHgtBias(now);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
PublishEvPosBias(now);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
|
@ -749,6 +763,8 @@ void EKF2::VerifyParams()
|
|||
"Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) {
|
||||
_param_ekf2_rng_ctrl.set(1);
|
||||
_param_ekf2_rng_ctrl.commit();
|
||||
|
|
@ -760,6 +776,8 @@ void EKF2::VerifyParams()
|
|||
"Range enabled by EKF2_HGT_REF", _param_ekf2_rng_ctrl.get());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
if ((_param_ekf2_hgt_ref.get() == HeightSensor::GNSS) && !(_param_ekf2_gps_ctrl.get() & GnssCtrl::VPOS)) {
|
||||
_param_ekf2_gps_ctrl.set(_param_ekf2_gps_ctrl.get() | (GnssCtrl::VPOS | GnssCtrl::HPOS | GnssCtrl::VEL));
|
||||
_param_ekf2_gps_ctrl.commit();
|
||||
|
|
@ -858,8 +876,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
|||
// baro height
|
||||
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// RNG height
|
||||
PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
// fake position
|
||||
PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
|
||||
|
|
@ -950,6 +970,7 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp)
|
|||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_ekf.get_rng_sample_delayed().time_us != 0) {
|
||||
|
|
@ -962,6 +983,7 @@ void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void EKF2::PublishEvPosBias(const hrt_abstime ×tamp)
|
||||
|
|
@ -1093,7 +1115,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
|||
const Vector3f position{_ekf.getPosition()};
|
||||
|
||||
// generate and publish global position data
|
||||
vehicle_global_position_s global_pos;
|
||||
vehicle_global_position_s global_pos{};
|
||||
global_pos.timestamp_sample = timestamp;
|
||||
|
||||
// Position of local NED origin in GPS / WGS84 frame
|
||||
|
|
@ -1118,16 +1140,19 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
|||
|
||||
_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
|
||||
|
||||
global_pos.terrain_alt = NAN;
|
||||
global_pos.terrain_alt_valid = false;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if (_ekf.isTerrainEstimateValid()) {
|
||||
// Terrain altitude in m, WGS84
|
||||
global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
|
||||
global_pos.terrain_alt_valid = true;
|
||||
|
||||
} else {
|
||||
global_pos.terrain_alt = NAN;
|
||||
global_pos.terrain_alt_valid = false;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
|
||||
|| _ekf.control_status_flags().wind_dead_reckoning;
|
||||
|
||||
|
|
@ -1181,7 +1206,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_ekf.getBaroHgtInnov(innovations.baro_vpos);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnov(innovations.rng_vpos);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnov(innovations.aux_hvel);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
|
@ -1200,8 +1227,10 @@ 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);
|
||||
// Not yet supported
|
||||
innovations.aux_vvel = NAN;
|
||||
|
|
@ -1230,7 +1259,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
|
||||
_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
|
||||
_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
|
||||
|
||||
|
|
@ -1249,7 +1280,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
|||
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
|
@ -1268,8 +1301,10 @@ 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]);
|
||||
// Not yet supported
|
||||
test_ratios.aux_vvel = NAN;
|
||||
|
|
@ -1288,7 +1323,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_ekf.getBaroHgtInnovVar(variances.baro_vpos);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovVar(variances.rng_vpos);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovVar(variances.aux_hvel);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
|
@ -1307,8 +1344,10 @@ 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)
|
||||
_ekf.getHaglInnovVar(variances.hagl);
|
||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
_ekf.getGravityInnovVar(variances.gravity);
|
||||
// Not yet supported
|
||||
variances.aux_vvel = NAN;
|
||||
|
|
@ -1319,7 +1358,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
|
||||
void EKF2::PublishLocalPosition(const hrt_abstime ×tamp)
|
||||
{
|
||||
vehicle_local_position_s lpos;
|
||||
vehicle_local_position_s lpos{};
|
||||
// generate vehicle local position data
|
||||
lpos.timestamp_sample = timestamp;
|
||||
|
||||
|
|
@ -1376,11 +1415,13 @@ 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_RANGE_FINDER)
|
||||
// Distance to bottom surface (ground) in meters
|
||||
// constrain the distance to ground to _rng_gnd_clearance
|
||||
lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get());
|
||||
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
|
||||
lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
|
||||
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
|
||||
|
|
@ -2228,6 +2269,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
distance_sensor_s distance_sensor;
|
||||
|
|
@ -2297,6 +2339,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
_distance_sensor_selected = -1;
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -64,7 +64,6 @@
|
|||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
#include <uORB/topics/estimator_bias.h>
|
||||
#include <uORB/topics/estimator_bias3d.h>
|
||||
|
|
@ -106,6 +105,10 @@
|
|||
# include <uORB/topics/vehicle_optical_flow_vel.h>
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
# include <uORB/topics/distance_sensor.h>
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
extern pthread_mutex_t ekf2_module_mutex;
|
||||
|
||||
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
|
||||
|
|
@ -153,7 +156,10 @@ private:
|
|||
void PublishAttitude(const hrt_abstime ×tamp);
|
||||
void PublishBaroBias(const hrt_abstime ×tamp);
|
||||
void PublishGnssHgtBias(const hrt_abstime ×tamp);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void PublishRngHgtBias(const hrt_abstime ×tamp);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void PublishEvPosBias(const hrt_abstime ×tamp);
|
||||
|
|
@ -194,7 +200,9 @@ private:
|
|||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
void UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
|
||||
// Used to check, save and use learned accel/gyro/mag biases
|
||||
|
|
@ -249,7 +257,6 @@ private:
|
|||
perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
|
||||
perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
|
||||
perf_counter_t _msg_missed_air_data_perf{nullptr};
|
||||
perf_counter_t _msg_missed_distance_sensor_perf{nullptr};
|
||||
perf_counter_t _msg_missed_gps_perf{nullptr};
|
||||
perf_counter_t _msg_missed_magnetometer_perf{nullptr};
|
||||
perf_counter_t _msg_missed_odometry_perf{nullptr};
|
||||
|
|
@ -376,10 +383,16 @@ private:
|
|||
uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
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)};
|
||||
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
hrt_abstime _last_range_sensor_update{0};
|
||||
int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
|
||||
unsigned _distance_sensor_last_generation{0};
|
||||
perf_counter_t _msg_missed_distance_sensor_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
bool _callback_registered{false};
|
||||
|
||||
|
|
@ -399,7 +412,6 @@ private:
|
|||
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
|
||||
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::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)};
|
||||
|
|
@ -412,7 +424,6 @@ private:
|
|||
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
|
||||
|
|
@ -453,8 +464,6 @@ private:
|
|||
_param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
|
||||
_param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
|
||||
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
|
||||
|
|
@ -477,9 +486,6 @@ private:
|
|||
_param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
|
||||
(ParamExtFloat<px4::params::EKF2_WIND_NSD>)
|
||||
_param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
|
||||
(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)
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
|
||||
_param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)
|
||||
|
|
@ -560,30 +566,47 @@ private:
|
|||
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
|
||||
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
|
||||
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl, ///< GPS control selection
|
||||
(ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl, ///< range finder control selection
|
||||
(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
|
||||
|
||||
(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_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)
|
||||
_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)
|
||||
_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)
|
||||
_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)
|
||||
_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)
|
||||
_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)
|
||||
_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)
|
||||
_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)
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// vision estimate fusion
|
||||
|
|
@ -642,9 +665,6 @@ private:
|
|||
(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_POS_Z>) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m)
|
||||
(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)
|
||||
|
||||
// output predictor filter time constants
|
||||
(ParamFloat<px4::params::EKF2_TAU_VEL>)
|
||||
|
|
|
|||
|
|
@ -60,9 +60,17 @@ menuconfig EKF2_OPTICAL_FLOW
|
|||
depends on MODULES_EKF2
|
||||
bool "optical flow fusion support"
|
||||
default y
|
||||
depends on EKF2_RANGE_FINDER
|
||||
---help---
|
||||
EKF2 optical flow fusion support.
|
||||
|
||||
menuconfig EKF2_RANGE_FINDER
|
||||
depends on MODULES_EKF2
|
||||
bool "range finder fusion support"
|
||||
default y
|
||||
---help---
|
||||
EKF2 range finder fusion support.
|
||||
|
||||
menuconfig EKF2_SIDESLIP
|
||||
depends on MODULES_EKF2
|
||||
bool "sideslip fusion support"
|
||||
|
|
|
|||
|
|
@ -117,11 +117,15 @@ bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s
|
|||
has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if (_is_using_rng_hgt_aiding) {
|
||||
const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
|
||||
has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
if (_is_using_ev_hgt_aiding) {
|
||||
const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
|
||||
has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
|
||||
|
|
@ -150,7 +154,6 @@ void PreFlightChecker::reset()
|
|||
_is_using_ev_vel_aiding = false;
|
||||
_is_using_baro_hgt_aiding = false;
|
||||
_is_using_gps_hgt_aiding = false;
|
||||
_is_using_rng_hgt_aiding = false;
|
||||
_is_using_ev_hgt_aiding = false;
|
||||
_has_heading_failed = false;
|
||||
_has_horiz_vel_failed = false;
|
||||
|
|
@ -161,10 +164,13 @@ void PreFlightChecker::reset()
|
|||
_filter_vel_d_innov.reset();
|
||||
_filter_baro_hgt_innov.reset();
|
||||
_filter_gps_hgt_innov.reset();
|
||||
_filter_rng_hgt_innov.reset();
|
||||
_filter_ev_hgt_innov.reset();
|
||||
_filter_heading_innov.reset();
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_is_using_rng_hgt_aiding = false;
|
||||
_filter_rng_hgt_innov.reset();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_is_using_flow_aiding = false;
|
||||
|
|
|
|||
|
|
@ -83,7 +83,9 @@ public:
|
|||
|
||||
void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
|
||||
void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
|
||||
|
||||
bool hasHeadingFailed() const { return _has_heading_failed; }
|
||||
|
|
@ -157,7 +159,6 @@ private:
|
|||
|
||||
bool _is_using_baro_hgt_aiding{};
|
||||
bool _is_using_gps_hgt_aiding{};
|
||||
bool _is_using_rng_hgt_aiding{};
|
||||
bool _is_using_ev_hgt_aiding{};
|
||||
|
||||
// Low-pass filters for innovation pre-flight checks
|
||||
|
|
@ -169,9 +170,13 @@ private:
|
|||
// Preflight low pass filter height innovation (m)
|
||||
InnovationLpf _filter_baro_hgt_innov;
|
||||
InnovationLpf _filter_gps_hgt_innov;
|
||||
InnovationLpf _filter_rng_hgt_innov;
|
||||
InnovationLpf _filter_ev_hgt_innov;
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
bool _is_using_rng_hgt_aiding {};
|
||||
InnovationLpf _filter_rng_hgt_innov;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
bool _is_using_flow_aiding {};
|
||||
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
|
||||
|
|
|
|||
Loading…
Reference in New Issue