mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: add kconfig option to enable/disable external vision fusion
This commit is contained in:
Daniel Agar
parent
d47f96f1a5
commit
4363b09421
@ -26,6 +26,7 @@ CONFIG_MODULES_EKF2=y
|
||||
# CONFIG_EKF2_AUXVEL is not set
|
||||
# CONFIG_EKF2_BARO_COMPENSATION is not set
|
||||
# CONFIG_EKF2_DRAG_FUSION is not set
|
||||
# CONFIG_EKF2_EXTERNAL_VISION is not set
|
||||
# CONFIG_EKF2_GNSS_YAW is not set
|
||||
# CONFIG_EKF2_SIDESLIP is not set
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
|
||||
@ -76,11 +76,6 @@ list(APPEND EKF_SRCS
|
||||
EKF/ekf_helper.cpp
|
||||
EKF/EKFGSF_yaw.cpp
|
||||
EKF/estimator_interface.cpp
|
||||
EKF/ev_control.cpp
|
||||
EKF/ev_height_control.cpp
|
||||
EKF/ev_pos_control.cpp
|
||||
EKF/ev_vel_control.cpp
|
||||
EKF/ev_yaw_control.cpp
|
||||
EKF/fake_height_control.cpp
|
||||
EKF/fake_pos_control.cpp
|
||||
EKF/gnss_height_control.cpp
|
||||
@ -115,6 +110,16 @@ if(CONFIG_EKF2_DRAG_FUSION)
|
||||
list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/ev_control.cpp
|
||||
EKF/ev_height_control.cpp
|
||||
EKF/ev_pos_control.cpp
|
||||
EKF/ev_vel_control.cpp
|
||||
EKF/ev_yaw_control.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_GNSS_YAW)
|
||||
list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
|
||||
endif()
|
||||
|
||||
@ -41,11 +41,6 @@ list(APPEND EKF_SRCS
|
||||
ekf_helper.cpp
|
||||
EKFGSF_yaw.cpp
|
||||
estimator_interface.cpp
|
||||
ev_control.cpp
|
||||
ev_height_control.cpp
|
||||
ev_pos_control.cpp
|
||||
ev_vel_control.cpp
|
||||
ev_yaw_control.cpp
|
||||
fake_height_control.cpp
|
||||
fake_pos_control.cpp
|
||||
gnss_height_control.cpp
|
||||
@ -80,6 +75,17 @@ if(CONFIG_EKF2_DRAG_FUSION)
|
||||
list(APPEND EKF_SRCS drag_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
list(APPEND EKF_SRCS
|
||||
ev_control.cpp
|
||||
ev_height_control.cpp
|
||||
ev_pos_control.cpp
|
||||
ev_vel_control.cpp
|
||||
ev_yaw_control.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
|
||||
if(CONFIG_EKF2_GNSS_YAW)
|
||||
list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
|
||||
endif()
|
||||
|
||||
@ -238,6 +238,7 @@ struct flowSample {
|
||||
uint8_t quality{}; ///< quality indicator between 0 and 255
|
||||
};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
struct extVisionSample {
|
||||
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
|
||||
Vector3f pos{}; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
|
||||
@ -251,6 +252,7 @@ struct extVisionSample {
|
||||
uint8_t reset_counter{};
|
||||
int8_t quality{}; ///< quality indicator between 0 and 100
|
||||
};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
struct dragSample {
|
||||
@ -299,7 +301,6 @@ struct parameters {
|
||||
int32_t baro_ctrl{1};
|
||||
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
|
||||
int32_t rng_ctrl{RngCtrl::CONDITIONAL};
|
||||
int32_t ev_ctrl{0};
|
||||
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
|
||||
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
|
||||
|
||||
@ -311,7 +312,6 @@ struct parameters {
|
||||
float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
|
||||
float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
||||
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
|
||||
float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
|
||||
|
||||
// input noise
|
||||
float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
|
||||
@ -395,7 +395,11 @@ struct parameters {
|
||||
float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
|
||||
float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// vision position fusion
|
||||
int32_t ev_ctrl{0};
|
||||
float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
|
||||
|
||||
float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec)
|
||||
float ev_pos_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m)
|
||||
float ev_att_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
|
||||
@ -403,6 +407,7 @@ struct parameters {
|
||||
float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
|
||||
float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
|
||||
float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// gravity fusion
|
||||
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
|
||||
|
||||
@ -122,8 +122,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
||||
controlHeightFusion(imu_delayed);
|
||||
controlGravityFusion(imu_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// Additional data odometry data from an external estimator can be fused.
|
||||
controlExternalVisionFusion();
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
// Additional horizontal velocity data from an auxiliary sensor can be fused
|
||||
|
||||
@ -444,14 +444,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
|
||||
if (fabsf(down_dvel_bias) > dVel_bias_lim) {
|
||||
|
||||
bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
|
||||
#else
|
||||
bool bad_vz_ev = false;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
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);
|
||||
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);
|
||||
bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f);
|
||||
|
||||
#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);
|
||||
#else
|
||||
bool bad_z_ev = false;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
|
||||
bad_acc_bias = true;
|
||||
|
||||
@ -126,10 +126,12 @@ void Ekf::reset()
|
||||
resetEstimatorAidStatus(_aid_src_fake_pos);
|
||||
resetEstimatorAidStatus(_aid_src_fake_hgt);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
resetEstimatorAidStatus(_aid_src_ev_hgt);
|
||||
resetEstimatorAidStatus(_aid_src_ev_pos);
|
||||
resetEstimatorAidStatus(_aid_src_ev_vel);
|
||||
resetEstimatorAidStatus(_aid_src_ev_yaw);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
resetEstimatorAidStatus(_aid_src_gnss_hgt);
|
||||
resetEstimatorAidStatus(_aid_src_gnss_pos);
|
||||
|
||||
@ -86,9 +86,11 @@ public:
|
||||
void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||
void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||
void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||
void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
|
||||
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
|
||||
@ -140,10 +142,12 @@ public:
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
heading_innov = _aid_src_ev_yaw.innovation;
|
||||
return;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
void getHeadingInnovVar(float &heading_innov_var) const
|
||||
@ -165,10 +169,12 @@ public:
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
heading_innov_var = _aid_src_ev_yaw.innovation_variance;
|
||||
return;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
void getHeadingInnovRatio(float &heading_innov_ratio) const
|
||||
@ -190,10 +196,12 @@ public:
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
|
||||
return;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
|
||||
@ -443,9 +451,11 @@ public:
|
||||
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(); }
|
||||
|
||||
const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
|
||||
@ -461,10 +471,12 @@ public:
|
||||
const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
|
||||
const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
const auto &aid_src_ev_hgt() const { return _aid_src_ev_hgt; }
|
||||
const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; }
|
||||
const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; }
|
||||
const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
|
||||
const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
|
||||
@ -525,9 +537,6 @@ private:
|
||||
|
||||
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
|
||||
|
||||
float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
|
||||
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
|
||||
|
||||
// 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
|
||||
@ -547,10 +556,6 @@ private:
|
||||
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
|
||||
uint64_t _time_last_healthy_rng_data{0};
|
||||
|
||||
uint8_t _nb_ev_pos_reset_available{0};
|
||||
uint8_t _nb_ev_vel_reset_available{0};
|
||||
uint8_t _nb_ev_yaw_reset_available{0};
|
||||
|
||||
Vector3f _last_known_pos{}; ///< last known local position vector (m)
|
||||
|
||||
uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec)
|
||||
@ -609,11 +614,20 @@ private:
|
||||
estimator_aid_source2d_s _aid_src_fake_pos{};
|
||||
estimator_aid_source1d_s _aid_src_fake_hgt{};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
estimator_aid_source1d_s _aid_src_ev_hgt{};
|
||||
estimator_aid_source2d_s _aid_src_ev_pos{};
|
||||
estimator_aid_source3d_s _aid_src_ev_vel{};
|
||||
estimator_aid_source1d_s _aid_src_ev_yaw{};
|
||||
|
||||
float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
|
||||
|
||||
uint8_t _nb_ev_pos_reset_available{0};
|
||||
uint8_t _nb_ev_vel_reset_available{0};
|
||||
uint8_t _nb_ev_yaw_reset_available{0};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
|
||||
|
||||
estimator_aid_source1d_s _aid_src_gnss_hgt{};
|
||||
estimator_aid_source2d_s _aid_src_gnss_pos{};
|
||||
estimator_aid_source3d_s _aid_src_gnss_vel{};
|
||||
@ -946,18 +960,21 @@ private:
|
||||
// Control the filter fusion modes
|
||||
void controlFusionModes(const imuSample &imu_delayed);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// control fusion of external vision observations
|
||||
void controlExternalVisionFusion();
|
||||
|
||||
void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
|
||||
|
||||
void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
|
||||
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
|
||||
void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
|
||||
|
||||
void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
|
||||
void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
|
||||
void stopEvPosFusion();
|
||||
|
||||
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
|
||||
void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
|
||||
void stopEvHgtFusion();
|
||||
void stopEvVelFusion();
|
||||
void stopEvYawFusion();
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// control fusion of optical flow observations
|
||||
void controlOpticalFlowFusion(const imuSample &imu_delayed);
|
||||
@ -1029,7 +1046,6 @@ private:
|
||||
void stopBaroHgtFusion();
|
||||
void stopGpsHgtFusion();
|
||||
void stopRngHgtFusion();
|
||||
void stopEvHgtFusion();
|
||||
|
||||
void updateGroundEffect();
|
||||
|
||||
@ -1095,9 +1111,6 @@ private:
|
||||
void stopGpsPosFusion();
|
||||
void stopGpsVelFusion();
|
||||
|
||||
void stopEvVelFusion();
|
||||
void stopEvYawFusion();
|
||||
|
||||
void stopFlowFusion();
|
||||
|
||||
void resetFakePosFusion();
|
||||
@ -1121,8 +1134,11 @@ 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};
|
||||
PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
|
||||
// Resets the horizontal velocity and position to the default navigation sensor
|
||||
|
||||
@ -150,7 +150,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
|
||||
|
||||
_state_reset_status.reset_count.posNE++;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
|
||||
|
||||
// Reset the timout timer
|
||||
@ -196,7 +198,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
|
||||
_state_reset_status.reset_count.posD++;
|
||||
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_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);
|
||||
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
|
||||
|
||||
@ -304,6 +308,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
|
||||
vpos = _aid_src_gnss_hgt.test_ratio;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
{
|
||||
hvel[0] = _aid_src_ev_vel.innovation[0];
|
||||
@ -334,6 +339,7 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp
|
||||
hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
|
||||
vpos = _aid_src_ev_hgt.test_ratio;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
|
||||
@ -466,9 +472,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_pos) {
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
*ekf_eph = hpos_err;
|
||||
@ -489,9 +497,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_pos) {
|
||||
hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
*ekf_eph = hpos_err;
|
||||
@ -516,14 +526,17 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_pos) {
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_pos) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm());
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm());
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
hvel_err = math::max(hvel_err, vel_err_conservative);
|
||||
}
|
||||
@ -637,6 +650,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio));
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// return the largest velocity and position innovation test ratio
|
||||
vel = NAN;
|
||||
pos = NAN;
|
||||
@ -649,6 +668,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
pos = math::max(gps_pos, FLT_MIN);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_vel) {
|
||||
float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max());
|
||||
vel = math::max(vel, ev_vel, FLT_MIN);
|
||||
@ -658,6 +678,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max());
|
||||
pos = math::max(pos, ev_pos, FLT_MIN);
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
|
||||
float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
|
||||
@ -683,10 +704,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
n_hgt_sources++;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio);
|
||||
n_hgt_sources++;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
if (n_hgt_sources > 0) {
|
||||
hgt = math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
|
||||
@ -1136,9 +1159,12 @@ bool Ekf::resetYawToEKFGSF()
|
||||
_control_status.flags.gps_yaw_fault = true;
|
||||
_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
|
||||
|
||||
} else if (_control_status.flags.ev_yaw) {
|
||||
}
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
_inhibit_ev_yaw_use = true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -54,7 +54,9 @@ EstimatorInterface::~EstimatorInterface()
|
||||
delete _airspeed_buffer;
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
delete _flow_buffer;
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
delete _ext_vision_buffer;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
delete _drag_buffer;
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
@ -355,7 +357,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
|
||||
}
|
||||
}
|
||||
|
||||
// set attitude and position data derived from an external vision system
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
|
||||
{
|
||||
if (!_initialised) {
|
||||
@ -392,6 +394,7 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
|
||||
ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
|
||||
@ -561,9 +564,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_params.ev_ctrl > 0) {
|
||||
max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
|
||||
|
||||
@ -703,9 +708,11 @@ void EstimatorInterface::print_status()
|
||||
printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_ext_vision_buffer) {
|
||||
printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
if (_drag_buffer) {
|
||||
|
||||
@ -99,8 +99,10 @@ public:
|
||||
// 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);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// set external vision position and attitude data
|
||||
void setExtVisionData(const extVisionSample &evdata);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
void setAuxVelData(const auxVelSample &auxvel_sample);
|
||||
@ -295,11 +297,17 @@ 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{};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
flowSample _flow_sample_delayed{};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
extVisionSample _ev_sample_prev{};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
RangeFinderConsistencyCheck _rng_consistency_check;
|
||||
|
||||
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
|
||||
@ -358,7 +366,11 @@ protected:
|
||||
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
RingBuffer<flowSample> *_flow_buffer{nullptr};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
|
||||
uint64_t _time_last_ext_vision_buffer_push{0};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
@ -368,7 +380,6 @@ protected:
|
||||
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_ext_vision_buffer_push{0};
|
||||
|
||||
uint64_t _time_last_gnd_effect_on{0};
|
||||
|
||||
|
||||
@ -178,9 +178,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
|
||||
|
||||
void Ekf::stopEvYawFusion()
|
||||
{
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
resetEstimatorAidStatus(_aid_src_ev_yaw);
|
||||
|
||||
_control_status.flags.ev_yaw = false;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
@ -119,6 +119,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
_aid_src_gnss_pos);
|
||||
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start
|
||||
if (_control_status.flags.tilt_align && _NED_origin_initialised
|
||||
&& gps_checks_passing && !gps_checks_failing) {
|
||||
@ -139,7 +140,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
@ -202,6 +203,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// Do not use external vision for yaw if using GPS because yaw needs to be
|
||||
// defined relative to an NED reference frame
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
@ -209,6 +211,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
stopEvYawFusion();
|
||||
_inhibit_ev_yaw_use = true;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
ECL_INFO("starting GPS fusion");
|
||||
_information_events.flags.starting_gps_fusion = true;
|
||||
@ -396,7 +399,7 @@ void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
|
||||
ECL_INFO("starting GPS yaw fusion");
|
||||
_control_status.flags.yaw_align = true;
|
||||
_control_status.flags.mag_dec = false;
|
||||
stopEvYawFusion();
|
||||
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
_control_status.flags.gps_yaw = true;
|
||||
|
||||
@ -166,11 +166,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
enum class ReferenceType { PRESSURE, GNSS, GROUND };
|
||||
|
||||
struct {
|
||||
ReferenceType ref_type;
|
||||
float innov;
|
||||
float innov_var;
|
||||
bool failed_min;
|
||||
bool failed_lim;
|
||||
ReferenceType ref_type{};
|
||||
float innov{0.f};
|
||||
float innov_var{0.f};
|
||||
bool failed_min{false};
|
||||
bool failed_lim{false};
|
||||
} checks[6] {};
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
@ -189,6 +189,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
checks[4] = {ReferenceType::GROUND, _aid_src_ev_hgt.innovation, _aid_src_ev_hgt.innovation_variance};
|
||||
}
|
||||
@ -196,6 +197,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
|
||||
if (_control_status.flags.ev_vel) {
|
||||
checks[5] = {ReferenceType::GROUND, _aid_src_ev_vel.innovation[2], _aid_src_ev_vel.innovation_variance[2]};
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// Compute the check based on innovation ratio for all the sources
|
||||
for (unsigned i = 0; i < 6; i++) {
|
||||
|
||||
@ -65,7 +65,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_gps_delay(_params->gps_delay_ms),
|
||||
_param_ekf2_of_delay(_params->flow_delay_ms),
|
||||
_param_ekf2_rng_delay(_params->range_delay_ms),
|
||||
_param_ekf2_ev_delay(_params->ev_delay_ms),
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
@ -132,6 +131,8 @@ 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),
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_param_ekf2_ev_delay(_params->ev_delay_ms),
|
||||
_param_ekf2_ev_ctrl(_params->ev_ctrl),
|
||||
_param_ekf2_ev_qmin(_params->ev_quality_minimum),
|
||||
_param_ekf2_evp_noise(_params->ev_pos_noise),
|
||||
@ -139,6 +140,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_eva_noise(_params->ev_att_noise),
|
||||
_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
|
||||
_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
|
||||
_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
|
||||
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
|
||||
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_param_ekf2_grav_noise(_params->gravity_noise),
|
||||
_param_ekf2_of_n_min(_params->flow_noise),
|
||||
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
|
||||
@ -156,9 +161,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
|
||||
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
|
||||
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
|
||||
_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
|
||||
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
|
||||
_param_ekf2_ev_pos_z(_params->ev_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),
|
||||
@ -240,6 +242,8 @@ bool EKF2::multi_init(int imu, int mag)
|
||||
_estimator_baro_bias_pub.advertise();
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
// EV advertise
|
||||
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS)) {
|
||||
_estimator_aid_src_ev_hgt_pub.advertise();
|
||||
@ -259,6 +263,8 @@ bool EKF2::multi_init(int imu, int mag)
|
||||
_estimator_aid_src_ev_yaw_pub.advertise();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// GNSS advertise
|
||||
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
|
||||
_estimator_aid_src_gnss_hgt_pub.advertise();
|
||||
@ -636,7 +642,9 @@ void EKF2::Run()
|
||||
UpdateAuxVelSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
UpdateBaroSample(ekf2_timestamps);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
UpdateExtVisionSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
UpdateFlowSample(ekf2_timestamps);
|
||||
UpdateGpsSample(ekf2_timestamps);
|
||||
UpdateMagSample(ekf2_timestamps);
|
||||
@ -658,7 +666,9 @@ void EKF2::Run()
|
||||
PublishBaroBias(now);
|
||||
PublishGnssHgtBias(now);
|
||||
PublishRngHgtBias(now);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
PublishEvPosBias(now);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
PublishEventFlags(now);
|
||||
PublishGpsStatus(now);
|
||||
PublishInnovations(now);
|
||||
@ -749,6 +759,8 @@ void EKF2::VerifyParams()
|
||||
"GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV)
|
||||
&& !(_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS))) {
|
||||
_param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast<int32_t>(EvCtrl::VPOS));
|
||||
@ -798,6 +810,7 @@ void EKF2::VerifyParams()
|
||||
"Use EKF2_EV_CTRL instead", _param_ekf2_aid_mask.get());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// IMU EKF2_AID_MASK -> EKF2_IMU_CTRL (2023-01-31)
|
||||
if (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_INHIBIT_ACC_BIAS) {
|
||||
@ -840,11 +853,13 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||
PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
|
||||
PublishAidSourceStatus(_ekf.aid_src_fake_hgt(), _status_fake_hgt_pub_last, _estimator_aid_src_fake_hgt_pub);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// external vision (EV) hgt/pos/vel/yaw
|
||||
PublishAidSourceStatus(_ekf.aid_src_ev_hgt(), _status_ev_hgt_pub_last, _estimator_aid_src_ev_hgt_pub);
|
||||
PublishAidSourceStatus(_ekf.aid_src_ev_pos(), _status_ev_pos_pub_last, _estimator_aid_src_ev_pos_pub);
|
||||
PublishAidSourceStatus(_ekf.aid_src_ev_vel(), _status_ev_vel_pub_last, _estimator_aid_src_ev_vel_pub);
|
||||
PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// GNSS hgt/pos/vel/yaw
|
||||
PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub);
|
||||
@ -934,6 +949,7 @@ void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp)
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void EKF2::PublishEvPosBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_ekf.aid_src_ev_hgt().timestamp_sample) {
|
||||
@ -966,6 +982,7 @@ void EKF2::PublishEvPosBias(const hrt_abstime ×tamp)
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
estimator_bias_s EKF2::fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
|
||||
uint64_t timestamp, uint32_t device_id)
|
||||
@ -1146,7 +1163,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
||||
estimator_innovations_s innovations{};
|
||||
innovations.timestamp_sample = _ekf.time_delayed_us();
|
||||
_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_ekf.getBaroHgtInnov(innovations.baro_vpos);
|
||||
_ekf.getRngHgtInnov(innovations.rng_vpos);
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
@ -1184,13 +1203,16 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
||||
// TODO: move to run before publications
|
||||
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
|
||||
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
|
||||
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
|
||||
_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
|
||||
_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
|
||||
_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
|
||||
_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
|
||||
|
||||
_preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
|
||||
|
||||
@ -1205,7 +1227,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
||||
test_ratios.timestamp_sample = _ekf.time_delayed_us();
|
||||
_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
|
||||
test_ratios.gps_vpos);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_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);
|
||||
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
@ -1240,7 +1264,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
||||
estimator_innovations_s variances{};
|
||||
variances.timestamp_sample = _ekf.time_delayed_us();
|
||||
_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
_ekf.getBaroHgtInnovVar(variances.baro_vpos);
|
||||
_ekf.getRngHgtInnovVar(variances.rng_vpos);
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
@ -1873,6 +1899,7 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
// EKF external vision sample
|
||||
@ -2026,6 +2053,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
|
||||
return new_ev_odom;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
|
||||
@ -151,7 +151,10 @@ private:
|
||||
void PublishBaroBias(const hrt_abstime ×tamp);
|
||||
void PublishGnssHgtBias(const hrt_abstime ×tamp);
|
||||
void PublishRngHgtBias(const hrt_abstime ×tamp);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void PublishEvPosBias(const hrt_abstime ×tamp);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
estimator_bias_s fillEstimatorBiasMsg(const BiasEstimator::status &status, uint64_t timestamp_sample_us,
|
||||
uint64_t timestamp, uint32_t device_id = 0);
|
||||
void PublishEventFlags(const hrt_abstime ×tamp);
|
||||
@ -278,11 +281,23 @@ private:
|
||||
hrt_abstime _status_fake_hgt_pub_last{0};
|
||||
hrt_abstime _status_fake_pos_pub_last{0};
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
|
||||
hrt_abstime _status_ev_hgt_pub_last{0};
|
||||
hrt_abstime _status_ev_pos_pub_last{0};
|
||||
hrt_abstime _status_ev_vel_pub_last{0};
|
||||
hrt_abstime _status_ev_yaw_pub_last{0};
|
||||
|
||||
matrix::Vector3f _last_ev_bias_published{};
|
||||
|
||||
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
|
||||
|
||||
uORB::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_bias)};
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
hrt_abstime _status_gnss_hgt_pub_last{0};
|
||||
hrt_abstime _status_gnss_pos_pub_last{0};
|
||||
hrt_abstime _status_gnss_vel_pub_last{0};
|
||||
@ -310,7 +325,6 @@ private:
|
||||
float _last_baro_bias_published{};
|
||||
float _last_gnss_hgt_bias_published{};
|
||||
float _last_rng_hgt_bias_published{};
|
||||
matrix::Vector3f _last_ev_bias_published{};
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
uORB::Subscription _airspeed_sub {ORB_ID(airspeed)};
|
||||
@ -336,7 +350,6 @@ private:
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
|
||||
uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
|
||||
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
@ -372,7 +385,6 @@ private:
|
||||
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::PublicationMulti<estimator_bias3d_s> _estimator_ev_pos_bias_pub{ORB_ID(estimator_ev_pos_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)};
|
||||
@ -391,11 +403,6 @@ private:
|
||||
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)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_hgt_pub{ORB_ID(estimator_aid_src_ev_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)};
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_yaw_pub{ORB_ID(estimator_aid_src_ev_yaw)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
|
||||
@ -439,8 +446,6 @@ private:
|
||||
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>)
|
||||
_param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
|
||||
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
|
||||
@ -571,21 +576,32 @@ private:
|
||||
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>)
|
||||
_param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD)
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// vision estimate fusion
|
||||
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
|
||||
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
|
||||
|
||||
(ParamExtInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection
|
||||
(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
|
||||
_param_ekf2_ev_noise_md, ///< determine source of vision observation noise
|
||||
(ParamInt<px4::params::EKF2_EV_NOISE_MD>) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
|
||||
(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
|
||||
(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)
|
||||
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
|
||||
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
|
||||
(ParamExtFloat<px4::params::EKF2_EVV_NOISE>)
|
||||
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
|
||||
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
|
||||
(ParamExtFloat<px4::params::EKF2_EVA_NOISE>)
|
||||
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
|
||||
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
|
||||
(ParamExtFloat<px4::params::EKF2_EVV_GATE>)
|
||||
_param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
|
||||
_param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
|
||||
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
|
||||
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
|
||||
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
|
||||
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
|
||||
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>)
|
||||
_param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2)
|
||||
@ -616,12 +632,6 @@ private:
|
||||
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
|
||||
_param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
|
||||
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
|
||||
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
|
||||
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
|
||||
_param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m)
|
||||
|
||||
// output predictor filter time constants
|
||||
(ParamFloat<px4::params::EKF2_TAU_VEL>)
|
||||
|
||||
@ -42,6 +42,13 @@ depends on MODULES_EKF2
|
||||
---help---
|
||||
EKF2 drag fusion support.
|
||||
|
||||
menuconfig EKF2_EXTERNAL_VISION
|
||||
depends on MODULES_EKF2
|
||||
bool "external vision (EV) fusion support"
|
||||
default y
|
||||
---help---
|
||||
EKF2 external vision (EV) fusion support.
|
||||
|
||||
menuconfig EKF2_GNSS_YAW
|
||||
depends on MODULES_EKF2
|
||||
bool "GNSS yaw fusion support"
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user