From d2b3e7fe161df80f98f47144856df2845acb9ea6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Oct 2023 14:02:34 -0400 Subject: [PATCH] ekf2: new kconfig to enable/disable GNSS (enabled by default) --- boards/bitcraze/crazyflie/default.px4board | 2 +- boards/bitcraze/crazyflie21/default.px4board | 2 +- src/modules/ekf2/CMakeLists.txt | 11 +- src/modules/ekf2/EKF/CMakeLists.txt | 11 +- src/modules/ekf2/EKF/common.h | 99 +++++----- src/modules/ekf2/EKF/control.cpp | 4 +- src/modules/ekf2/EKF/covariance.cpp | 19 +- src/modules/ekf2/EKF/ekf.cpp | 13 +- src/modules/ekf2/EKF/ekf.h | 178 +++++++++--------- src/modules/ekf2/EKF/ekf_helper.cpp | 41 +++- src/modules/ekf2/EKF/estimator_interface.cpp | 8 + src/modules/ekf2/EKF/estimator_interface.h | 53 +++--- src/modules/ekf2/EKF/ev_height_control.cpp | 2 + src/modules/ekf2/EKF/ev_pos_control.cpp | 2 + src/modules/ekf2/EKF/ev_vel_control.cpp | 2 + src/modules/ekf2/EKF/ev_yaw_control.cpp | 5 +- src/modules/ekf2/EKF/fake_pos_control.cpp | 2 +- src/modules/ekf2/EKF/gps_checks.cpp | 10 + src/modules/ekf2/EKF/gps_control.cpp | 8 +- src/modules/ekf2/EKF/height_control.cpp | 4 + src/modules/ekf2/EKF/optical_flow_control.cpp | 24 ++- src/modules/ekf2/EKF2.cpp | 132 +++++++++---- src/modules/ekf2/EKF2.hpp | 167 ++++++++-------- src/modules/ekf2/Kconfig | 8 + 24 files changed, 476 insertions(+), 331 deletions(-) diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 326273dda2..e7e9705f2d 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -15,7 +15,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y -# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_GNSS is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index bd99fc6964..aa55b50621 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -14,7 +14,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y -# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_GNSS is not set # CONFIG_EKF2_MAGNETOMETER is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_EVENTS=y diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index d0a796c315..8f10bae93c 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -124,9 +124,6 @@ list(APPEND EKF_SRCS EKF/estimator_interface.cpp EKF/fake_height_control.cpp EKF/fake_pos_control.cpp - EKF/gnss_height_control.cpp - EKF/gps_checks.cpp - EKF/gps_control.cpp EKF/gravity_fusion.cpp EKF/height_control.cpp EKF/imu_down_sampler.cpp @@ -166,6 +163,14 @@ if(CONFIG_EKF2_EXTERNAL_VISION) ) endif() +if(CONFIG_EKF2_GNSS) + list(APPEND EKF_SRCS + EKF/gnss_height_control.cpp + EKF/gps_checks.cpp + EKF/gps_control.cpp + ) +endif() + if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 98fb6e02ee..23ebe86cbd 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -42,9 +42,6 @@ list(APPEND EKF_SRCS estimator_interface.cpp fake_height_control.cpp fake_pos_control.cpp - gnss_height_control.cpp - gps_checks.cpp - gps_control.cpp gravity_fusion.cpp height_control.cpp imu_down_sampler.cpp @@ -84,6 +81,14 @@ if(CONFIG_EKF2_EXTERNAL_VISION) ) endif() +if(CONFIG_EKF2_GNSS) + list(APPEND EKF_SRCS + gnss_height_control.cpp + gps_checks.cpp + gps_control.cpp + ) +endif() + if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 39aa79ba3d..81137ad240 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -280,15 +280,9 @@ struct parameters { // measurement source control int32_t height_sensor_ref{HeightSensor::BARO}; int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; - int32_t baro_ctrl{1}; - int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; 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) - // measurement time delays - 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) - // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) float accel_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2) @@ -308,18 +302,66 @@ struct parameters { float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) +#if defined(CONFIG_EKF2_BAROMETER) + int32_t baro_ctrl{1}; + float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) + float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) + float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) + float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) + + float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) + float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) + +# if defined(CONFIG_EKF2_BARO_COMPENSATION) + // static barometer pressure position error coefficient along body axes + float static_pressure_coef_xp{0.0f}; // (-) + float static_pressure_coef_xn{0.0f}; // (-) + float static_pressure_coef_yp{0.0f}; // (-) + float static_pressure_coef_yn{0.0f}; // (-) + float static_pressure_coef_z{0.0f}; // (-) + + // upper limit on airspeed used for correction (m/s**2) + float max_correction_airspeed{20.0f}; +# endif // CONFIG_EKF2_BARO_COMPENSATION +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_GNSS) + int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; + float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) + + Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) + // position and velocity fusion float gps_vel_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m) float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz)) - float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) - float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) - float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) - float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) float gps_pos_innov_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD) float gps_vel_innov_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD) - float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) - float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) + + // these parameters control the strictness of GPS quality checks used to determine if the GPS is + // good enough to set a local origin and commence aiding + int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used + float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m) + float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m) + float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s) + int32_t req_nsats{6}; ///< minimum acceptable satellite count + float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision + float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s) + float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s) + +# if defined(CONFIG_EKF2_GNSS_YAW) + // GNSS heading fusion + float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) +# endif // CONFIG_EKF2_GNSS_YAW + + // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value + float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) + const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value + const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) + +#endif // CONFIG_EKF2_GNSS + + float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD) float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) @@ -346,11 +388,6 @@ struct parameters { float mag_check_inclination_tolerance_deg{20.f}; #endif // CONFIG_EKF2_MAGNETOMETER -#if defined(CONFIG_EKF2_GNSS_YAW) - // GNSS heading fusion - float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) -#endif // CONFIG_EKF2_GNSS_YAW - #if defined(CONFIG_EKF2_AIRSPEED) // airspeed fusion float airspeed_delay_ms{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec) @@ -434,20 +471,8 @@ struct parameters { 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 - // good enough to set a local origin and commence aiding - int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used - float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m) - float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m) - float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s) - int32_t req_nsats{6}; ///< minimum acceptable satellite count - float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision - float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s) - float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s) - // 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) // accel bias learning control float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2) @@ -463,18 +488,6 @@ struct parameters { int32_t valid_timeout_max{5'000'000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) -#if defined(CONFIG_EKF2_BARO_COMPENSATION) - // static barometer pressure position error coefficient along body axes - float static_pressure_coef_xp{0.0f}; // (-) - float static_pressure_coef_xn{0.0f}; // (-) - float static_pressure_coef_yp{0.0f}; // (-) - float static_pressure_coef_yn{0.0f}; // (-) - float static_pressure_coef_z{0.0f}; // (-) - - // upper limit on airspeed used for correction (m/s**2) - float max_correction_airspeed {20.0f}; -#endif // CONFIG_EKF2_BARO_COMPENSATION - #if defined(CONFIG_EKF2_DRAG_FUSION) // multi-rotor drag specific force fusion int32_t drag_ctrl{0}; @@ -496,10 +509,6 @@ struct parameters { const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) #endif // CONFIG_EKF2_AUXVEL - // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value - float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) - const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value - const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) }; union fault_status_u { diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index a1ad5f40f3..704c3e4350 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -92,7 +92,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) } ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", - (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); + (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); ECL_DEBUG("tilt aligned, roll: %.3f, pitch %.3f, yaw: %.3f", (double)matrix::Eulerf(_state.quat_nominal).phi(), @@ -111,7 +111,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlOpticalFlowFusion(imu_delayed); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) controlGpsFusion(imu_delayed); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AIRSPEED) controlAirDataFusion(imu_delayed); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 5960087a52..28572f69de 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -57,16 +57,24 @@ void Ekf::initialiseCovariance() resetQuatCov(); // velocity +#if defined(CONFIG_EKF2_GNSS) const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f)); +#else + const float vel_var = sq(0.5f); +#endif P.uncorrelateCovarianceSetVariance(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var)); // position - const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f)); float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f)); +#if defined(CONFIG_EKF2_GNSS) + const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f)); if (_control_status.flags.gps_hgt) { z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); } +#else + const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f)); +#endif #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { @@ -404,8 +412,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation bool bad_acc_bias = false; if (fabsf(down_dvel_bias) > dVel_bias_lim) { - +#if defined(CONFIG_EKF2_GNSS) bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f); +#else + bool bad_vz_gps = false; +#endif // CONFIG_EKF2_GNSS #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 @@ -418,7 +429,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) #else bool bad_z_baro = false; #endif +#if defined(CONFIG_EKF2_GNSS) bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f); +#else + bool bad_z_gps = false; +#endif #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); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 4937e9abb8..9d2ac69309 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -92,7 +92,11 @@ void Ekf::reset() _prev_gyro_bias_var.zero(); _prev_accel_bias_var.zero(); +#if defined(CONFIG_EKF2_GNSS) resetGpsDriftCheckFilters(); + _gps_checks_passed = false; +#endif // CONFIG_EKF2_GNSS + _gps_alt_ref = NAN; _output_predictor.reset(); @@ -112,9 +116,6 @@ void Ekf::reset() _time_acc_bias_check = 0; - _gps_checks_passed = false; - _gps_alt_ref = NAN; - #if defined(CONFIG_EKF2_BAROMETER) _baro_counter = 0; #endif // CONFIG_EKF2_BAROMETER @@ -147,13 +148,15 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_ev_yaw); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) resetEstimatorAidStatus(_aid_src_gnss_hgt); resetEstimatorAidStatus(_aid_src_gnss_pos); resetEstimatorAidStatus(_aid_src_gnss_vel); -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) resetEstimatorAidStatus(_aid_src_gnss_yaw); -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) resetEstimatorAidStatus(_aid_src_mag_heading); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 2afa901e81..c1df5e4f3b 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -80,10 +80,6 @@ public: static uint8_t getNumberOfStates() { return State::size; } - void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; - 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; @@ -343,19 +339,11 @@ public: Vector3f getVelocityVariance() const { return getStateVariance(); }; Vector3f getPositionVariance() const { return getStateVariance(); } - - // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined - bool collect_gps(const gpsMessage &gps) override; - // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude); - float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } - bool setEkfGlobalOriginAltitude(const float altitude); - - // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; @@ -373,14 +361,6 @@ public: void resetGyroBias(); void resetAccelBias(); - // First argument returns GPS drift metrics in the following array locations - // 0 : Horizontal position drift rate (m/s) - // 1 : Vertical position drift rate (m/s) - // 2 : Filtered horizontal velocity (m/s) - // Second argument returns true when IMU movement is blocking the drift calculation - // Function returns true if the metrics have been updated and not returned previously by this function - bool get_gps_drift_metrics(float drift[3], bool *blocked); - // return true if the global position estimate is valid // return true if the origin is set we are not doing unconstrained free inertial navigation // and have not started using synthetic position observations to constrain drift @@ -504,26 +484,11 @@ public: Vector3f calcRotVecVariances() const; float getYawVar() const; - // set minimum continuous period without GPS fail required to mark a healthy GPS status - void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } - - const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; } - const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; } - - bool gps_checks_passed() const { return _gps_checks_passed; }; - - // get solution data from the EKF-GSF emergency yaw estimator - // returns false when data is not available - bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], - float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); - // Returns true if the output of the yaw emergency estimator can be used for a reset bool isYawEmergencyEstimateAvailable() const; uint8_t getHeightSensorRef() const { return _height_sensor_ref; } - const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } - #if defined(CONFIG_EKF2_EXTERNAL_VISION) const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); } @@ -548,13 +513,37 @@ public: const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; } #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) + void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; + void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; + void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; + + // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined + bool collect_gps(const gpsMessage &gps) override; + + // set minimum continuous period without GPS fail required to mark a healthy GPS status + void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } + + const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; } + const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; } + + bool gps_checks_passed() const { return _gps_checks_passed; }; + + // get solution data from the EKF-GSF emergency yaw estimator + // returns false when data is not available + bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], + float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); + + const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } + const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; } const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; } const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; } -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; } -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } @@ -606,9 +595,6 @@ private: bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised - // 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 - 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}; uint64_t _time_last_v_vel_aiding{0}; @@ -710,20 +696,9 @@ private: uint8_t _nb_ev_yaw_reset_available{0}; #endif // CONFIG_EKF2_EXTERNAL_VISION - estimator_aid_source1d_s _aid_src_gnss_hgt{}; - estimator_aid_source2d_s _aid_src_gnss_pos{}; - estimator_aid_source3d_s _aid_src_gnss_vel{}; - -#if defined(CONFIG_EKF2_GNSS_YAW) - estimator_aid_source1d_s _aid_src_gnss_yaw{}; - uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source -#endif // CONFIG_EKF2_GNSS_YAW - - estimator_aid_source3d_s _aid_src_gravity{}; - -#if defined(CONFIG_EKF2_AUXVEL) - estimator_aid_source2d_s _aid_src_aux_vel{}; -#endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_GNSS) + // 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 // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) @@ -736,8 +711,27 @@ private: uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed - // Variables used to publish the WGS-84 location of the EKF local NED origin - float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + gps_check_fail_status_u _gps_check_fail_status{}; + // height sensor status + bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent + + HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; + + estimator_aid_source1d_s _aid_src_gnss_hgt{}; + estimator_aid_source2d_s _aid_src_gnss_pos{}; + estimator_aid_source3d_s _aid_src_gnss_vel{}; + +# if defined(CONFIG_EKF2_GNSS_YAW) + estimator_aid_source1d_s _aid_src_gnss_yaw{}; + uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS + + estimator_aid_source3d_s _aid_src_gravity{}; + +#if defined(CONFIG_EKF2_AUXVEL) + estimator_aid_source2d_s _aid_src_aux_vel{}; +#endif // CONFIG_EKF2_AUXVEL // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; @@ -785,8 +779,6 @@ private: Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2) #endif // CONFIG_EKF2_MAGNETOMETER - gps_check_fail_status_u _gps_check_fail_status{}; - // variables used to inhibit accel bias learning bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis bool _gyro_bias_inhibit[3] {}; ///< true when the gyro bias learning is being inhibited for the specified axis @@ -797,9 +789,6 @@ private: Vector3f _prev_gyro_bias_var{}; ///< saved gyro XYZ bias variances Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances - // height sensor status - bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent - // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) @@ -818,7 +807,8 @@ private: void predictCovariance(const imuSample &imu_delayed); template - void resetStateCovariance(const matrix::SquareMatrix &cov) { + void resetStateCovariance(const matrix::SquareMatrix &cov) + { P.uncorrelateCovarianceSetVariance(S.idx, 0.0f); P.slice(S.idx, S.idx) = cov; } @@ -828,21 +818,6 @@ private: bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const; -#if defined(CONFIG_EKF2_GNSS_YAW) - void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); - - // fuse the yaw angle obtained from a dual antenna GPS unit - void fuseGpsYaw(); - - // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit - // return true if the reset was successful - bool resetYawToGps(const float gnss_yaw); - - void updateGpsYaw(const gpsSample &gps_sample); - -#endif // CONFIG_EKF2_GNSS_YAW - void stopGpsYawFusion(); - #if defined(CONFIG_EKF2_MAGNETOMETER) // ekf sequential fusion of magnetometer measurements bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); @@ -1079,9 +1054,6 @@ private: // calculate the earth rotation vector from a given latitude Vector3f calcEarthRateNED(float lat_rad) const; - // return true id the GPS quality is good enough to set an origin and start aiding - bool gps_is_good(const gpsMessage &gps); - // Control the filter fusion modes void controlFusionModes(const imuSample &imu_delayed); @@ -1102,11 +1074,43 @@ private: void stopEvYawFusion(); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) // control fusion of GPS observations void controlGpsFusion(const imuSample &imu_delayed); + void stopGpsFusion(); + bool shouldResetGpsFusion() const; bool isYawFailure() const; + // return true id the GPS quality is good enough to set an origin and start aiding + bool gps_is_good(const gpsMessage &gps); + + void controlGnssHeightFusion(const gpsSample &gps_sample); + void stopGpsHgtFusion(); + + // 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 + // Returns true if the reset was successful + bool resetYawToEKFGSF(); + + void resetGpsDriftCheckFilters(); + +# if defined(CONFIG_EKF2_GNSS_YAW) + void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); + void stopGpsYawFusion(); + + // fuse the yaw angle obtained from a dual antenna GPS unit + void fuseGpsYaw(); + + // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit + // return true if the reset was successful + bool resetYawToGps(const float gnss_yaw); + + void updateGpsYaw(const gpsSample &gps_sample); + +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations void controlMagFusion(); @@ -1165,7 +1169,6 @@ private: // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(const imuSample &imu_delayed); void checkHeightSensorRefFallback(); - void controlGnssHeightFusion(const gpsSample &gps_sample); #if defined(CONFIG_EKF2_BAROMETER) void controlBaroHeightFusion(); @@ -1174,8 +1177,6 @@ private: void updateGroundEffect(); #endif // CONFIG_EKF2_BAROMETER - void stopGpsHgtFusion(); - // gravity fusion: heuristically enable / disable gravity fusion void controlGravityFusion(const imuSample &imu_delayed); @@ -1212,8 +1213,6 @@ private: return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us); } - void stopGpsFusion(); - void resetFakePosFusion(); void stopFakePosFusion(); @@ -1232,8 +1231,6 @@ private: uint8_t _height_sensor_ref{HeightSensor::UNKNOWN}; uint8_t _position_sensor_ref{static_cast(PositionSensor::GNSS)}; - HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _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(PositionSensor::EV), _position_sensor_ref}; @@ -1241,13 +1238,6 @@ private: bool _ev_q_error_initialized{false}; #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 - // Returns true if the reset was successful - bool resetYawToEKFGSF(); - - void resetGpsDriftCheckFilters(); - void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const { // only bother resetting if timestamp_sample is set diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ef6596bb00..cf27006ccf 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -206,7 +206,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v #if defined(CONFIG_EKF2_EXTERNAL_VISION) _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); #endif // CONFIG_EKF2_RANGE_FINDER @@ -293,6 +295,7 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)); } +#if defined(CONFIG_EKF2_GNSS) void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const { hvel[0] = _aid_src_gnss_vel.innovation[0]; @@ -323,6 +326,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]); vpos = _aid_src_gnss_hgt.test_ratio; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const @@ -440,11 +444,15 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons // determine current z float current_alt = -_state.pos(2) + gps_alt_ref_prev; +#if defined(CONFIG_EKF2_GNSS) const float gps_hgt_bias = _gps_hgt_b_est.getBias(); +#endif // CONFIG_EKF2_GNSS resetVerticalPositionTo(_gps_alt_ref - current_alt); +#if defined(CONFIG_EKF2_GNSS) // preserve GPS height bias _gps_hgt_b_est.setBias(gps_hgt_bias); +#endif // CONFIG_EKF2_GNSS } return true; @@ -465,9 +473,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations if (_control_status.flags.inertial_dead_reckoning) { +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { @@ -490,9 +500,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations if (_horizontal_deadreckon_time_exceeded) { +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { @@ -523,9 +535,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { @@ -670,6 +684,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f vel = NAN; pos = NAN; +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max()); vel = math::max(gps_vel, FLT_MIN); @@ -677,6 +692,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max()); pos = math::max(gps_pos, FLT_MIN); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_vel) { @@ -708,10 +724,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f } #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps_hgt) { hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio); n_hgt_sources++; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { @@ -795,10 +813,15 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const } #endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_GNSS) const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; +#else + (void)mag_innov_good; +#endif // CONFIG_EKF2_GNSS + soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical; *status = soln_status.value; } @@ -995,6 +1018,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _time_last_heading_fuse = _time_delayed_us; } +#if defined(CONFIG_EKF2_GNSS) bool Ekf::resetYawToEKFGSF() { if (!isYawEmergencyEstimateAvailable()) { @@ -1019,9 +1043,11 @@ bool Ekf::resetYawToEKFGSF() return true; } +#endif // CONFIG_EKF2_GNSS bool Ekf::isYawEmergencyEstimateAvailable() const { +#if defined(CONFIG_EKF2_GNSS) // don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity // data and the yaw estimate has converged if (!_yawEstimator.isActive()) { @@ -1029,23 +1055,18 @@ bool Ekf::isYawEmergencyEstimateAvailable() const } return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); +#else + return false; +#endif } +#if defined(CONFIG_EKF2_GNSS) bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) { return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight); } - -void Ekf::resetGpsDriftCheckFilters() -{ - _gps_velNE_filt.setZero(); - _gps_pos_deriv_filt.setZero(); - - _gps_horizontal_position_drift_rate_m_s = NAN; - _gps_vertical_position_drift_rate_m_s = NAN; - _gps_filtered_horizontal_velocity_m_s = NAN; -} +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_WIND) void Ekf::resetWind() diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 3063ba077f..bf97dc2a75 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -46,7 +46,9 @@ EstimatorInterface::~EstimatorInterface() { +#if defined(CONFIG_EKF2_GNSS) delete _gps_buffer; +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) delete _mag_buffer; #endif // CONFIG_EKF2_MAGNETOMETER @@ -144,6 +146,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) } #endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_GNSS) void EstimatorInterface::setGpsData(const gpsMessage &gps) { if (!_initialised) { @@ -222,6 +225,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) void EstimatorInterface::setBaroData(const baroSample &baro_sample) @@ -585,9 +589,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) if (_params.gnss_ctrl > 0) { max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_params.flow_ctrl > 0) { @@ -713,9 +719,11 @@ void EstimatorInterface::print_status() printf("minimum observation interval %d us\n", _min_obs_interval_us); +#if defined(CONFIG_EKF2_GNSS) if (_gps_buffer) { printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) if (_mag_buffer) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index a9e7f06f1a..5143a10c2d 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -81,17 +81,24 @@ using namespace estimator; class EstimatorInterface { public: + void setIMUData(const imuSample &imu_sample); + +#if defined(CONFIG_EKF2_GNSS) // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined virtual bool collect_gps(const gpsMessage &gps) = 0; + void setGpsData(const gpsMessage &gps); - void setIMUData(const imuSample &imu_sample); + const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } + + float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } + float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } + float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) void setMagData(const magSample &mag_sample); #endif // CONFIG_EKF2_MAGNETOMETER - void setGpsData(const gpsMessage &gps); - #if defined(CONFIG_EKF2_BAROMETER) void setBaroData(const baroSample &baro_sample); #endif // CONFIG_EKF2_BAROMETER @@ -299,17 +306,12 @@ public: const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } const uint64_t &time_delayed_us() const { return _time_delayed_us; } - const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } - const bool &global_origin_valid() const { return _NED_origin_initialised; } const MapProjection &global_origin() const { return _pos_ref; } + float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } void print_status(); - float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; } - float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; } - float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; } - OutputPredictor &output_predictor() { return _output_predictor; }; protected: @@ -345,10 +347,6 @@ protected: OutputPredictor _output_predictor{}; - // measurement samples capturing measurements on the delayed time horizon - gpsSample _gps_sample_delayed{}; - - #if defined(CONFIG_EKF2_AIRSPEED) airspeedSample _airspeed_sample_delayed{}; #endif // CONFIG_EKF2_AIRSPEED @@ -381,18 +379,33 @@ protected: 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 + // Variables used to publish the WGS-84 location of the EKF local NED origin bool _NED_origin_initialised{false}; + MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin + float _gps_alt_ref{NAN}; ///< WGS-84 height (m) float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin - MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin + +#if defined(CONFIG_EKF2_GNSS) + RingBuffer *_gps_buffer{nullptr}; + uint64_t _time_last_gps_buffer_push{0}; + + gpsSample _gps_sample_delayed{}; + + float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) + float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) + float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) + MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) -#if defined(CONFIG_EKF2_GNSS_YAW) + +# if defined(CONFIG_EKF2_GNSS_YAW) float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians). // innovation consistency check monitoring ratios AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state uint64_t _time_last_gps_yaw_buffer_push{0}; -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_DRAG_FUSION) RingBuffer *_drag_buffer{nullptr}; @@ -405,10 +418,6 @@ protected: bool _vertical_position_deadreckon_time_exceeded{true}; bool _vertical_velocity_deadreckon_time_exceeded{true}; - float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) - float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) - float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) - uint64_t _time_last_on_ground_us{0}; ///< last time we were on the ground (uSec) uint64_t _time_last_in_air{0}; ///< last time we were in air (uSec) @@ -416,8 +425,6 @@ protected: static constexpr uint8_t kBufferLengthDefault = 12; RingBuffer _imu_buffer{kBufferLengthDefault}; - RingBuffer *_gps_buffer{nullptr}; - #if defined(CONFIG_EKF2_MAGNETOMETER) RingBuffer *_mag_buffer{nullptr}; uint64_t _time_last_mag_buffer_push{0}; @@ -436,8 +443,6 @@ protected: #endif // CONFIG_EKF2_AUXVEL RingBuffer *_system_flag_buffer{nullptr}; - uint64_t _time_last_gps_buffer_push{0}; - #if defined(CONFIG_EKF2_BAROMETER) RingBuffer *_baro_buffer{nullptr}; uint64_t _time_last_baro_buffer_push{0}; diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index 4ad4dd7e0f..0a4bc00c84 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -76,10 +76,12 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com const float measurement = pos(2) - pos_offset_earth(2); float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f)); +#if defined(CONFIG_EKF2_GNSS) // increase minimum variance if GPS active if (_control_status.flags.gps_hgt) { measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise)); } +#endif // CONFIG_EKF2_GNSS const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index e969af287c..3f6027400a 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -125,12 +125,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common break; } +#if defined(CONFIG_EKF2_GNSS) // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise)); } } +#endif // CONFIG_EKF2_GNSS const Vector2f measurement{pos(0), pos(1)}; diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index a2431689d9..aff345ac11 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -106,12 +106,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common break; } +#if defined(CONFIG_EKF2_GNSS) // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); } } +#endif // CONFIG_EKF2_GNSS const Vector3f measurement{vel}; diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index d7d4eee92a..3afb3b78fc 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -155,10 +155,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common _control_status.flags.ev_yaw = true; } else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) { - // turn on fusion of external vision yaw measurements and disable all other heading fusion - stopGpsYawFusion(); - stopGpsFusion(); - + // turn on fusion of external vision yaw measurements ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); // reset yaw to EV diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index 5ff14f8862..4962cd55fc 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -52,7 +52,7 @@ void Ekf::controlFakePosFusion() Vector2f obs_var; if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise)); + obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, 1.f)); } else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) { // Accelerate tilt fine alignment by fusing more diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 62f63467b5..7396b31f3c 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -271,3 +271,13 @@ bool Ekf::gps_is_good(const gpsMessage &gps) // continuous period without fail of x seconds required to return a healthy status return isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us); } + +void Ekf::resetGpsDriftCheckFilters() +{ + _gps_velNE_filt.setZero(); + _gps_pos_deriv_filt.setZero(); + + _gps_horizontal_position_drift_rate_m_s = NAN; + _gps_vertical_position_drift_rate_m_s = NAN; + _gps_filtered_horizontal_velocity_m_s = NAN; +} diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 4c620de17f..2f25153ccc 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -400,12 +400,9 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi stopGpsYawFusion(); } } -#endif // CONFIG_EKF2_GNSS_YAW void Ekf::stopGpsYawFusion() { -#if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { _control_status.flags.gps_yaw = false; @@ -421,9 +418,8 @@ void Ekf::stopGpsYawFusion() ECL_INFO("stopping GPS yaw fusion"); } } - -#endif // CONFIG_EKF2_GNSS_YAW } +#endif // CONFIG_EKF2_GNSS_YAW void Ekf::stopGpsFusion() { @@ -436,5 +432,7 @@ void Ekf::stopGpsFusion() } stopGpsHgtFusion(); +#if defined(CONFIG_EKF2_GNSS_YAW) stopGpsYawFusion(); +#endif // CONFIG_EKF2_GNSS_YAW } diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 6f7a3305b2..d5c3542897 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -47,7 +47,9 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) controlBaroHeightFusion(); #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) controlGnssHeightFusion(_gps_sample_delayed); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) controlRangeHeightFusion(); @@ -185,6 +187,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const } #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps_hgt) { checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance}; } @@ -192,6 +195,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const if (_control_status.flags.gps) { checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]}; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index a8207db178..d4b2dc1466 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -142,15 +142,25 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon if (_flow_data_ready) { - // Inhibit flow use if motion is un-suitable or we have good quality GPS - // Apply hysteresis to prevent rapid mode switching - const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f; - // Check if we are in-air and require optical flow to control position drift - const bool is_flow_required = _control_status.flags.in_air + bool is_flow_required = _control_status.flags.in_air && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow) - || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad + || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)); + +#if defined(CONFIG_EKF2_GNSS) + // check if using GPS, but GPS is bad + if (_control_status.flags.gps) { + if (_control_status.flags.in_air && !is_flow_required) { + // Inhibit flow use if motion is un-suitable or we have good quality GPS + // Apply hysteresis to prevent rapid mode switching + const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f; + + if (_gps_error_norm > gps_err_norm_lim) { + is_flow_required = true; + } + } + } +#endif // CONFIG_EKF2_GNSS // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation const bool preflight_motion_not_ok = !_control_status.flags.in_air diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 53eeeee925..291e9aa64d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -62,7 +62,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->filter_update_interval_us), _param_ekf2_imu_ctrl(_params->imu_ctrl), - _param_ekf2_gps_delay(_params->gps_delay_ms), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), #endif // CONFIG_EKF2_AUXVEL @@ -73,9 +72,27 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): #if defined(CONFIG_EKF2_WIND) _param_ekf2_wind_nsd(_params->wind_vel_nsd), #endif // CONFIG_EKF2_WIND + _param_ekf2_noaid_noise(_params->pos_noaid_noise), +#if defined(CONFIG_EKF2_GNSS) + _param_ekf2_gps_ctrl(_params->gnss_ctrl), + _param_ekf2_gps_delay(_params->gps_delay_ms), + _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_gps_v_noise(_params->gps_vel_noise), _param_ekf2_gps_p_noise(_params->gps_pos_noise), - _param_ekf2_noaid_noise(_params->pos_noaid_noise), + _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), + _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate), + _param_ekf2_gps_check(_params->gps_check_mask), + _param_ekf2_req_eph(_params->req_hacc), + _param_ekf2_req_epv(_params->req_vacc), + _param_ekf2_req_sacc(_params->req_sacc), + _param_ekf2_req_nsats(_params->req_nsats), + _param_ekf2_req_pdop(_params->req_pdop), + _param_ekf2_req_hdrift(_params->req_hdrift), + _param_ekf2_req_vdrift(_params->req_vdrift), + _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default), +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) _param_ekf2_baro_ctrl(_params->baro_ctrl), _param_ekf2_baro_delay(_params->baro_delay_ms), @@ -92,8 +109,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_pcoef_z(_params->static_pressure_coef_z), # endif // CONFIG_EKF2_BARO_COMPENSATION #endif // CONFIG_EKF2_BAROMETER - _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), - _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate), #if defined(CONFIG_EKF2_AIRSPEED) _param_ekf2_asp_delay(_params->airspeed_delay_ms), _param_ekf2_tas_gate(_params->tas_innov_gate), @@ -123,16 +138,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg), _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), #endif // CONFIG_EKF2_MAGNETOMETER - _param_ekf2_gps_check(_params->gps_check_mask), - _param_ekf2_req_eph(_params->req_hacc), - _param_ekf2_req_epv(_params->req_vacc), - _param_ekf2_req_sacc(_params->req_sacc), - _param_ekf2_req_nsats(_params->req_nsats), - _param_ekf2_req_pdop(_params->req_pdop), - _param_ekf2_req_hdrift(_params->req_hdrift), - _param_ekf2_req_vdrift(_params->req_vdrift), _param_ekf2_hgt_ref(_params->height_sensor_ref), - _param_ekf2_gps_ctrl(_params->gnss_ctrl), _param_ekf2_noaid_tout(_params->valid_timeout_max), #if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) _param_ekf2_min_rng(_params->rng_gnd_clearance), @@ -171,7 +177,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _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), #if defined(CONFIG_EKF2_OPTICAL_FLOW) _param_ekf2_of_ctrl(_params->flow_ctrl), _param_ekf2_of_delay(_params->flow_delay_ms), @@ -187,9 +192,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_imu_pos_x(_params->imu_pos_body(0)), _param_ekf2_imu_pos_y(_params->imu_pos_body(1)), _param_ekf2_imu_pos_z(_params->imu_pos_body(2)), - _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_gbias_init(_params->switch_on_gyro_bias), _param_ekf2_abias_init(_params->switch_on_accel_bias), _param_ekf2_angerr_init(_params->initial_tilt_err), @@ -205,7 +207,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_bcoef_y(_params->bcoef_y), _param_ekf2_mcoef(_params->mcoef), #endif // CONFIG_EKF2_DRAG_FUSION - _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) + _param_ekf2_grav_noise(_params->gravity_noise) + { // advertise expected minimal topic set immediately to ensure logging _attitude_pub.advertise(); @@ -233,7 +236,9 @@ EKF2::~EKF2() #if defined(CONFIG_EKF2_RANGE_FINDER) perf_free(_msg_missed_distance_sensor_perf); #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) perf_free(_msg_missed_gps_perf); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AUXVEL) perf_free(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL @@ -262,7 +267,9 @@ bool EKF2::multi_init(int imu, int mag) _estimator_states_pub.advertise(); _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); +#if defined(CONFIG_EKF2_GNSS) _yaw_est_pub.advertise(); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) @@ -297,6 +304,8 @@ bool EKF2::multi_init(int imu, int mag) #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) + // GNSS advertise if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { _estimator_aid_src_gnss_hgt_pub.advertise(); @@ -312,13 +321,14 @@ bool EKF2::multi_init(int imu, int mag) _estimator_aid_src_gnss_vel_pub.advertise(); } -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::YAW)) { _estimator_aid_src_gnss_yaw_pub.advertise(); } -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -395,7 +405,9 @@ int EKF2::print_status() #if defined(CONFIG_EKF2_RANGE_FINDER) perf_print_counter(_msg_missed_distance_sensor_perf); #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) perf_print_counter(_msg_missed_gps_perf); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AUXVEL) perf_print_counter(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL @@ -434,7 +446,9 @@ void EKF2::Run() VerifyParams(); +#if defined(CONFIG_EKF2_GNSS) _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); +#endif // CONFIG_EKF2_GNSS const matrix::Vector3f imu_pos_body(_param_ekf2_imu_pos_x.get(), _param_ekf2_imu_pos_y.get(), @@ -721,7 +735,9 @@ void EKF2::Run() #if defined(CONFIG_EKF2_OPTICAL_FLOW) UpdateFlowSample(ekf2_timestamps); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) UpdateGpsSample(ekf2_timestamps); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagSample(ekf2_timestamps); #endif // CONFIG_EKF2_MAGNETOMETER @@ -739,43 +755,49 @@ void EKF2::Run() PublishLocalPosition(now); PublishOdometry(now, imu_sample_new); PublishGlobalPosition(now); + PublishSensorBias(now); #if defined(CONFIG_EKF2_WIND) PublishWindEstimate(now); #endif // CONFIG_EKF2_WIND // publish status/logging messages -#if defined(CONFIG_EKF2_BAROMETER) - PublishBaroBias(now); -#endif // CONFIG_EKF2_BAROMETER - 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 PublishEventFlags(now); - PublishGpsStatus(now); PublishInnovations(now); PublishInnovationTestRatios(now); PublishInnovationVariances(now); -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - PublishOpticalFlowVel(now); -#endif // CONFIG_EKF2_OPTICAL_FLOW PublishStates(now); PublishStatus(now); PublishStatusFlags(now); + PublishAidSourceStatus(now); + +#if defined(CONFIG_EKF2_BAROMETER) + PublishBaroBias(now); +#endif // CONFIG_EKF2_BAROMETER + +#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 + +#if defined(CONFIG_EKF2_GNSS) + PublishGnssHgtBias(now); + PublishGpsStatus(now); PublishYawEstimatorStatus(now); +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + PublishOpticalFlowVel(now); +#endif // CONFIG_EKF2_OPTICAL_FLOW UpdateAccelCalibration(now); UpdateGyroCalibration(now); #if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagCalibration(now); #endif // CONFIG_EKF2_MAGNETOMETER - PublishSensorBias(now); - - PublishAidSourceStatus(now); } else { // ekf no update @@ -792,6 +814,8 @@ void EKF2::Run() void EKF2::VerifyParams() { +#if defined(CONFIG_EKF2_GNSS) + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS) || (_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_GPS_YAW)) { _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_GPS | @@ -816,6 +840,8 @@ void EKF2::VerifyParams() "GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get()); } +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_BAROMETER) if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) { @@ -846,6 +872,8 @@ void EKF2::VerifyParams() #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_GNSS) + 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(); @@ -857,6 +885,8 @@ void EKF2::VerifyParams() "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get()); } +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_EXTERNAL_VISION) if ((_param_ekf2_hgt_ref.get() == HeightSensor::EV) @@ -1034,13 +1064,15 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_ev_yaw(), _status_ev_yaw_pub_last, _estimator_aid_src_ev_yaw_pub); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) // GNSS hgt/pos/vel/yaw PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub); PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub); PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub); -#if defined(CONFIG_EKF2_GNSS_YAW) +# if defined(CONFIG_EKF2_GNSS_YAW) PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub); -#endif // CONFIG_EKF2_GNSS_YAW +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) // mag heading @@ -1115,6 +1147,7 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) } #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) { if (_ekf.get_gps_sample_delayed().time_us != 0) { @@ -1127,6 +1160,7 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) @@ -1280,7 +1314,11 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) _ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon); global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters +#if defined(CONFIG_EKF2_GNSS) global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt); +#else + global_pos.alt_ellipsoid = global_pos.alt; +#endif // delta_alt, alt_reset_counter // global altitude has opposite sign of local down position @@ -1319,6 +1357,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_GNSS) void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) { const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us; @@ -1353,13 +1392,16 @@ void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) _last_gps_status_published = timestamp_sample; } +#endif // CONFIG_EKF2_GNSS void EKF2::PublishInnovations(const hrt_abstime ×tamp) { // publish estimator innovation data estimator_innovations_s innovations{}; innovations.timestamp_sample = _ekf.time_delayed_us(); +#if defined(CONFIG_EKF2_GNSS) _ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos); +#endif // CONFIG_EKF2_GNSS #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 @@ -1449,8 +1491,10 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) // publish estimator innovation test ratio data estimator_innovations_s test_ratios{}; test_ratios.timestamp_sample = _ekf.time_delayed_us(); +#if defined(CONFIG_EKF2_GNSS) _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], test_ratios.gps_vpos); +#endif // CONFIG_EKF2_GNSS #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 @@ -1504,7 +1548,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) // publish estimator innovation variance data estimator_innovations_s variances{}; variances.timestamp_sample = _ekf.time_delayed_us(); +#if defined(CONFIG_EKF2_GNSS) _ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos); +#endif // CONFIG_EKF2_GNSS #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 @@ -1787,9 +1833,11 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _ekf.getOutputTrackingError().copyTo(status.output_tracking_error); +#if defined(CONFIG_EKF2_GNSS) // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include // the GPS Fix bit, which is always checked) status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1); +#endif // CONFIG_EKF2_GNSS status.control_mode_flags = _ekf.control_status().value; status.filter_fault_flags = _ekf.fault_status().value; @@ -1948,6 +1996,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_GNSS) void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) { static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF, @@ -1967,6 +2016,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp) _yaw_est_pub.publish(yaw_est_test_data); } } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_WIND) void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) @@ -2029,6 +2079,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) float EKF2::filter_altitude_ellipsoid(float amsl_hgt) { float height_diff = static_cast(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt; @@ -2049,6 +2100,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt) return amsl_hgt + _wgs84_hgt_offset; } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_AIRSPEED) void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) @@ -2412,6 +2464,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_GNSS) void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF GPS message @@ -2455,6 +2508,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) _gps_alttitude_ellipsoid = static_cast(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)); } } +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index b40f535e7d..2465b774cc 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include @@ -80,7 +79,6 @@ #include #include #include -#include #include #include #include @@ -101,6 +99,11 @@ # include #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_GNSS) +# include +# include +#endif // CONFIG_EKF2_GNSS + #if defined(CONFIG_EKF2_MAGNETOMETER) # include #endif // CONFIG_EKF2_MAGNETOMETER @@ -180,7 +183,6 @@ private: #if defined(CONFIG_EKF2_BAROMETER) void PublishBaroBias(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_BAROMETER - void PublishGnssHgtBias(const hrt_abstime ×tamp); #if defined(CONFIG_EKF2_RANGE_FINDER) void PublishRngHgtBias(const hrt_abstime ×tamp); @@ -193,15 +195,11 @@ private: uint64_t timestamp, uint32_t device_id = 0); void PublishEventFlags(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); - void PublishGpsStatus(const hrt_abstime ×tamp); void PublishInnovations(const hrt_abstime ×tamp); void PublishInnovationTestRatios(const hrt_abstime ×tamp); void PublishInnovationVariances(const hrt_abstime ×tamp); void PublishLocalPosition(const hrt_abstime ×tamp); void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample); -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - void PublishOpticalFlowVel(const hrt_abstime ×tamp); -#endif // CONFIG_EKF2_OPTICAL_FLOW void PublishSensorBias(const hrt_abstime ×tamp); void PublishStates(const hrt_abstime ×tamp); void PublishStatus(const hrt_abstime ×tamp); @@ -209,7 +207,6 @@ private: #if defined(CONFIG_EKF2_WIND) void PublishWindEstimate(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_WIND - void PublishYawEstimatorStatus(const hrt_abstime ×tamp); #if defined(CONFIG_EKF2_AIRSPEED) void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); @@ -223,16 +220,28 @@ private: #if defined(CONFIG_EKF2_EXTERNAL_VISION) bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_GNSS) + /* + * Calculate filtered WGS84 height from estimated AMSL height + */ + float filter_altitude_ellipsoid(float amsl_hgt); + + void PublishGpsStatus(const hrt_abstime ×tamp); + void PublishGnssHgtBias(const hrt_abstime ×tamp); + void PublishYawEstimatorStatus(const hrt_abstime ×tamp); + void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_OPTICAL_FLOW) bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); + void PublishOpticalFlowVel(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_OPTICAL_FLOW - void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); #if defined(CONFIG_EKF2_MAGNETOMETER) void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_MAGNETOMETER #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 @@ -267,11 +276,6 @@ private: } } - /* - * Calculate filtered WGS84 height from estimated AMSL height - */ - float filter_altitude_ellipsoid(float amsl_hgt); - static constexpr float sq(float x) { return x * x; }; const bool _replay_mode{false}; ///< true when we use replay data from a log @@ -288,17 +292,11 @@ private: perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; 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_gps_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; InFlightCalibration _accel_cal{}; InFlightCalibration _gyro_cal{}; - uint64_t _gps_time_usec{0}; - int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid - uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt - float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 - uint8_t _accel_calibration_count{0}; uint8_t _gyro_calibration_count{0}; @@ -309,7 +307,6 @@ private: Vector3f _last_gyro_bias_published{}; hrt_abstime _last_sensor_bias_published{0}; - hrt_abstime _last_gps_status_published{0}; hrt_abstime _status_fake_hgt_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0}; @@ -352,13 +349,6 @@ private: uORB::PublicationMulti _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}; -#if defined(CONFIG_EKF2_GNSS_YAW) - hrt_abstime _status_gnss_yaw_pub_last {0}; -#endif // CONFIG_EKF2_GNSS_YAW - hrt_abstime _status_gravity_pub_last{0}; #if defined(CONFIG_EKF2_AUXVEL) @@ -413,8 +403,6 @@ private: hrt_abstime _status_drag_pub_last{0}; #endif // CONFIG_EKF2_DRAG_FUSION - float _last_gnss_hgt_bias_published{}; - #if defined(CONFIG_EKF2_AIRSPEED) uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; @@ -441,7 +429,6 @@ private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; - uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; @@ -477,9 +464,7 @@ private: uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; - uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; @@ -487,18 +472,10 @@ private: uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; - uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; - uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; - uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; - uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; -#if defined(CONFIG_EKF2_GNSS_YAW) - uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; -#endif // CONFIG_EKF2_GNSS_YAW - uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; // publications with topic dependent on multi-mode @@ -511,6 +488,36 @@ private: uORB::PublicationMulti _wind_pub; #endif // CONFIG_EKF2_WIND +#if defined(CONFIG_EKF2_GNSS) + perf_counter_t _msg_missed_gps_perf {nullptr}; + + uint64_t _gps_time_usec{0}; + int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid + uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt + float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 + + hrt_abstime _last_gps_status_published{0}; + + hrt_abstime _status_gnss_hgt_pub_last{0}; + hrt_abstime _status_gnss_pos_pub_last{0}; + hrt_abstime _status_gnss_vel_pub_last{0}; + + float _last_gnss_hgt_bias_published{}; + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + + uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; + uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; + uORB::PublicationMulti _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)}; + uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; + uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; + uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; +# if defined(CONFIG_EKF2_GNSS_YAW) + hrt_abstime _status_gnss_yaw_pub_last {0}; + uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; +# endif // CONFIG_EKF2_GNSS_YAW +#endif // CONFIG_EKF2_GNSS + PreFlightChecker _preflt_checker; Ekf _ekf; @@ -521,9 +528,6 @@ private: (ParamExtInt) _param_ekf2_predict_us, (ParamExtInt) _param_ekf2_imu_ctrl, - (ParamExtFloat) - _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) - #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) _param_ekf2_avel_delay, ///< auxiliary velocity measurement delay relative to the IMU (mSec) @@ -544,12 +548,35 @@ private: (ParamExtFloat) _param_ekf2_wind_nsd, #endif // CONFIG_EKF2_WIND - (ParamExtFloat) - _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) - (ParamExtFloat) - _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m) - (ParamExtFloat) - _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) + (ParamExtFloat) _param_ekf2_noaid_noise, + +#if defined(CONFIG_EKF2_GNSS) + (ParamExtInt) _param_ekf2_gps_ctrl, + (ParamExtFloat) _param_ekf2_gps_delay, + + (ParamExtFloat) _param_ekf2_gps_pos_x, + (ParamExtFloat) _param_ekf2_gps_pos_y, + (ParamExtFloat) _param_ekf2_gps_pos_z, + + (ParamExtFloat) _param_ekf2_gps_v_noise, + (ParamExtFloat) _param_ekf2_gps_p_noise, + + (ParamExtFloat) _param_ekf2_gps_p_gate, + (ParamExtFloat) _param_ekf2_gps_v_gate, + + (ParamExtInt) _param_ekf2_gps_check, + (ParamExtFloat) _param_ekf2_req_eph, + (ParamExtFloat) _param_ekf2_req_epv, + (ParamExtFloat) _param_ekf2_req_sacc, + (ParamExtInt) _param_ekf2_req_nsats, + (ParamExtFloat) _param_ekf2_req_pdop, + (ParamExtFloat) _param_ekf2_req_hdrift, + (ParamExtFloat) _param_ekf2_req_vdrift, + (ParamFloat) _param_ekf2_req_gps_h, + + // Used by EKF-GSF experimental yaw estimator + (ParamExtFloat) _param_ekf2_gsf_tas_default, +#endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection @@ -570,11 +597,6 @@ private: # endif // CONFIG_EKF2_BARO_COMPENSATION #endif // CONFIG_EKF2_BAROMETER - (ParamExtFloat) - _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_gps_v_gate, ///< GPS velocity innovation consistency gate size (STD) - #if defined(CONFIG_EKF2_AIRSPEED) (ParamExtFloat) _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) @@ -616,23 +638,10 @@ private: (ParamExtInt) _param_ekf2_synthetic_mag_z, #endif // CONFIG_EKF2_MAGNETOMETER - (ParamExtInt) - _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used - (ParamExtFloat) _param_ekf2_req_eph, ///< maximum acceptable horiz position error (m) - (ParamExtFloat) _param_ekf2_req_epv, ///< maximum acceptable vert position error (m) - (ParamExtFloat) _param_ekf2_req_sacc, ///< maximum acceptable speed error (m/s) - (ParamExtInt) _param_ekf2_req_nsats, ///< minimum acceptable satellite count - (ParamExtFloat) - _param_ekf2_req_pdop, ///< maximum acceptable position dilution of precision - (ParamExtFloat) - _param_ekf2_req_hdrift, ///< maximum acceptable horizontal drift speed (m/s) - (ParamExtFloat) _param_ekf2_req_vdrift, ///< maximum acceptable vertical drift speed (m/s) - // measurement source control (ParamInt) _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used (ParamExtInt) _param_ekf2_hgt_ref, ///< selects the primary source for height data - (ParamExtInt) _param_ekf2_gps_ctrl, ///< GPS control selection (ParamExtInt) _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) @@ -689,10 +698,6 @@ private: (ParamExtFloat) _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) #endif // CONFIG_EKF2_EXTERNAL_VISION - - (ParamExtFloat) - _param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2) - #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow fusion (ParamExtInt) @@ -721,15 +726,6 @@ private: (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m) (ParamExtFloat) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_x, ///< X position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_y, ///< Y position of GPS antenna in body frame (m) - (ParamExtFloat) _param_ekf2_gps_pos_z, ///< Z position of GPS antenna in body frame (m) - - // output predictor filter time constants - (ParamFloat) - _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) - (ParamFloat) - _param_ekf2_tau_pos, ///< time constant used by the output position complementary filter (sec) // IMU switch on bias parameters (ParamExtFloat) @@ -760,12 +756,11 @@ private: (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) #endif // CONFIG_EKF2_DRAG_FUSION - (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time - - // Used by EKF-GSF experimental yaw estimator - (ParamExtFloat) - _param_ekf2_gsf_tas_default ///< default value of true airspeed assumed during fixed wing operation + // output predictor filter time constants + (ParamFloat) _param_ekf2_tau_vel, + (ParamFloat) _param_ekf2_tau_pos, + (ParamExtFloat) _param_ekf2_grav_noise ) }; #endif // !EKF2_HPP diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index fc47a4f3d7..4a39f9d422 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -60,10 +60,18 @@ depends on MODULES_EKF2 ---help--- EKF2 external vision (EV) fusion support. +menuconfig EKF2_GNSS +depends on MODULES_EKF2 + bool "GNSS fusion support" + default y + ---help--- + EKF2 GNSS fusion support. + menuconfig EKF2_GNSS_YAW depends on MODULES_EKF2 bool "GNSS yaw fusion support" default y + depends on EKF2_GNSS ---help--- EKF2 GNSS yaw fusion support.