diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index a407eb27fe..326273dda2 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -15,6 +15,8 @@ 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_SIDESLIP is not set CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 9f2e37f8f1..a5e5244cc8 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -14,6 +14,8 @@ 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_SIDESLIP is not set CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 40512a0c22..22f72e357d 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -26,6 +26,7 @@ CONFIG_MODULES_EKF2=y # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set # CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOGGER=y diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index bb5fa66e09..8e9a394aab 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -68,7 +68,6 @@ add_subdirectory(Utility) set(EKF_SRCS) list(APPEND EKF_SRCS - EKF/airspeed_fusion.cpp EKF/auxvel_fusion.cpp EKF/baro_height_control.cpp EKF/bias_estimator.cpp @@ -99,13 +98,16 @@ list(APPEND EKF_SRCS EKF/range_finder_consistency_check.cpp EKF/range_height_control.cpp EKF/sensor_range_finder.cpp - EKF/sideslip_fusion.cpp EKF/terrain_estimator.cpp EKF/vel_pos_fusion.cpp EKF/zero_innovation_heading_update.cpp EKF/zero_velocity_update.cpp ) +if(CONFIG_EKF2_AIRSPEED) + list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS EKF/drag_fusion.cpp) endif() @@ -114,6 +116,10 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_SIDESLIP) + list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) +endif() + px4_add_module( MODULE modules__ekf2 MAIN ekf2 diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 9009008100..d143baf090 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -33,7 +33,6 @@ set(EKF_SRCS) list(APPEND EKF_SRCS - airspeed_fusion.cpp auxvel_fusion.cpp baro_height_control.cpp bias_estimator.cpp @@ -64,13 +63,16 @@ list(APPEND EKF_SRCS range_finder_consistency_check.cpp range_height_control.cpp sensor_range_finder.cpp - sideslip_fusion.cpp terrain_estimator.cpp vel_pos_fusion.cpp zero_innovation_heading_update.cpp zero_velocity_update.cpp ) +if(CONFIG_EKF2_AIRSPEED) + list(APPEND EKF_SRCS airspeed_fusion.cpp) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS drag_fusion.cpp) endif() @@ -79,6 +81,10 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_SIDESLIP) + list(APPEND EKF_SRCS sideslip_fusion.cpp) +endif() + add_library(ecl_EKF ${EKF_SRCS} ) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 2dd2e97dcd..c609169ce2 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -222,21 +222,6 @@ void Ekf::stopAirspeedFusion() } } -float Ekf::getTrueAirspeed() const -{ - return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm(); -} - -void Ekf::resetWind() -{ - if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) { - resetWindUsingAirspeed(_airspeed_sample_delayed); - - } else { - resetWindToZero(); - } -} - void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample) { const float euler_yaw = getEulerYaw(_R_to_earth); @@ -282,14 +267,3 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample P(22, 22) += P(4, 4); P(23, 23) += P(5, 5); } - -void Ekf::resetWindToZero() -{ - ECL_INFO("reset wind to zero"); - - // If we don't have an airspeed measurement, then assume the wind is zero - _state.wind_vel.setZero(); - - // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty); -} diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 8126d1b559..3b3baed475 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -307,7 +307,6 @@ struct parameters { float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) - float airspeed_delay_ms{100.0f}; ///< airspeed 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) @@ -364,16 +363,21 @@ struct parameters { 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) float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) float eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s) float arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) // synthetic sideslip fusion int32_t beta_fusion_enabled{0}; float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD) float beta_noise{0.3f}; ///< synthetic sideslip noise (rad) const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) +#endif // CONFIG_EKF2_SIDESLIP // range finder fusion float range_noise{0.1f}; ///< observation noise for range finder measurements (m) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 9573cd752c..cb48c0ada0 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -106,8 +106,14 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlMagFusion(); controlOpticalFlowFusion(imu_delayed); controlGpsFusion(imu_delayed); + +#if defined(CONFIG_EKF2_AIRSPEED) controlAirDataFusion(imu_delayed); +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) controlBetaFusion(imu_delayed); +#endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_DRAG_FUSION) controlDragFusion(); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 4786cd79db..d5e16b3dd7 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -116,8 +116,12 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_baro_hgt); resetEstimatorAidStatus(_aid_src_rng_hgt); +#if defined(CONFIG_EKF2_AIRSPEED) resetEstimatorAidStatus(_aid_src_airspeed); +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) resetEstimatorAidStatus(_aid_src_sideslip); +#endif // CONFIG_EKF2_SIDESLIP resetEstimatorAidStatus(_aid_src_fake_pos); resetEstimatorAidStatus(_aid_src_fake_hgt); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 3f3332fb69..b715e7189a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -204,13 +204,17 @@ public: void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); } #endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_AIRSPEED) void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; } void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; } void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; } +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; } void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; } void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; } +#endif // CONFIG_EKF2_SIDESLIP void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } @@ -233,9 +237,6 @@ public: // get the wind velocity var Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); } - // get the true airspeed in m/s - float getTrueAirspeed() const; - // get the full covariance matrix const matrix::SquareMatrix &covariances() const { return P; } @@ -444,8 +445,13 @@ public: const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); } +#if defined(CONFIG_EKF2_AIRSPEED) const auto &aid_src_airspeed() const { return _aid_src_airspeed; } +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) const auto &aid_src_sideslip() const { return _aid_src_sideslip; } +#endif // CONFIG_EKF2_SIDESLIP const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } @@ -589,8 +595,12 @@ private: estimator_aid_source1d_s _aid_src_baro_hgt{}; estimator_aid_source1d_s _aid_src_rng_hgt{}; +#if defined(CONFIG_EKF2_AIRSPEED) estimator_aid_source1d_s _aid_src_airspeed{}; +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) estimator_aid_source1d_s _aid_src_sideslip{}; +#endif // CONFIG_EKF2_SIDESLIP estimator_aid_source2d_s _aid_src_fake_pos{}; estimator_aid_source1d_s _aid_src_fake_hgt{}; @@ -725,12 +735,30 @@ private: // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); +#if defined(CONFIG_EKF2_AIRSPEED) + // control fusion of air data observations + void controlAirDataFusion(const imuSample &imu_delayed); + void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const; void fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src); + void stopAirspeedFusion(); + + // Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip + void resetWindUsingAirspeed(const airspeedSample &airspeed_sample); + + // perform a limited reset of the wind state covariances + void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample); +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) + // control fusion of synthetic sideslip observations + void controlBetaFusion(const imuSample &imu_delayed); + // fuse synthetic zero sideslip measurement void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const; void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); +#endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_DRAG_FUSION) // control fusion of multi-rotor drag specific force observations @@ -956,12 +984,6 @@ private: void runMagAndMagDeclFusions(const Vector3f &mag); void run3DMagAndDeclFusions(const Vector3f &mag); - // control fusion of air data observations - void controlAirDataFusion(const imuSample &imu_delayed); - - // control fusion of synthetic sideslip observations - void controlBetaFusion(const imuSample &imu_delayed); - // control fusion of fake position observations to constrain drift void controlFakePosFusion(); @@ -1021,13 +1043,6 @@ private: // perform a reset of the wind states and related covariances void resetWind(); - - // Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip - void resetWindUsingAirspeed(const airspeedSample &airspeed_sample); - - // perform a limited reset of the wind state covariances - void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample); - void resetWindToZero(); // check that the range finder data is continuous @@ -1067,8 +1082,6 @@ private: return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us); } - void stopAirspeedFusion(); - void stopGpsFusion(); void stopGpsPosFusion(); void stopGpsVelFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 60b027078b..abf695fd8f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -693,14 +693,18 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f hgt = NAN; } +#if defined(CONFIG_EKF2_AIRSPEED) // return the airspeed fusion innovation test ratio tas = sqrtf(_aid_src_airspeed.test_ratio); +#endif // CONFIG_EKF2_AIRSPEED // return the terrain height innovation test ratio hagl = sqrtf(_hagl_test_ratio); +#if defined(CONFIG_EKF2_SIDESLIP) // return the synthetic sideslip innovation test ratio beta = sqrtf(_aid_src_sideslip.test_ratio); +#endif // CONFIG_EKF2_SIDESLIP } // return a bitmask integer that describes which state estimates are valid @@ -775,11 +779,18 @@ void Ekf::updateHorizontalDeadReckoningstatus() const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max); - const bool airDataAiding = _control_status.flags.wind && + bool airDataAiding = false; + +#if defined(CONFIG_EKF2_AIRSPEED) + airDataAiding = _control_status.flags.wind && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max); _control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; +#else + _control_status.flags.wind_dead_reckoning = false; +#endif // CONFIG_EKF2_AIRSPEED + _control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; if (!_control_status.flags.inertial_dead_reckoning) { @@ -1156,3 +1167,26 @@ void Ekf::resetGpsDriftCheckFilters() _gps_vertical_position_drift_rate_m_s = NAN; _gps_filtered_horizontal_velocity_m_s = NAN; } + +void Ekf::resetWind() +{ +#if defined(CONFIG_EKF2_AIRSPEED) + if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) { + resetWindUsingAirspeed(_airspeed_sample_delayed); + return; + } +#endif // CONFIG_EKF2_AIRSPEED + + resetWindToZero(); +} + +void Ekf::resetWindToZero() +{ + ECL_INFO("reset wind to zero"); + + // If we don't have an airspeed measurement, then assume the wind is zero + _state.wind_vel.setZero(); + + // start with a small initial uncertainty to improve the initial estimate + P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty); +} diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index f514778720..e6b25b7d7b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -50,7 +50,9 @@ EstimatorInterface::~EstimatorInterface() delete _mag_buffer; delete _baro_buffer; delete _range_buffer; +#if defined(CONFIG_EKF2_AIRSPEED) delete _airspeed_buffer; +#endif // CONFIG_EKF2_AIRSPEED delete _flow_buffer; delete _ext_vision_buffer; #if defined(CONFIG_EKF2_DRAG_FUSION) @@ -243,6 +245,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) } } +#if defined(CONFIG_EKF2_AIRSPEED) void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) { if (!_initialised) { @@ -277,6 +280,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_AIRSPEED void EstimatorInterface::setRangeData(const rangeSample &range_sample) { @@ -523,10 +527,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms); } +#if defined(CONFIG_EKF2_AIRSPEED) // using airspeed if (_params.arsp_thr > FLT_EPSILON) { max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_AIRSPEED // mag mode if (_params.mag_fusion_type != MagFuseType::NONE) { @@ -678,9 +684,11 @@ void EstimatorInterface::print_status() printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size()); } +#if defined(CONFIG_EKF2_AIRSPEED) if (_airspeed_buffer) { printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size()); } +#endif // CONFIG_EKF2_AIRSPEED if (_flow_buffer) { printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size()); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 6055bb9dd4..da58f3a224 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -90,7 +90,9 @@ public: void setBaroData(const baroSample &baro_sample); +#if defined(CONFIG_EKF2_AIRSPEED) void setAirspeedData(const airspeedSample &airspeed_sample); +#endif // CONFIG_EKF2_AIRSPEED void setRangeData(const rangeSample &range_sample); @@ -291,7 +293,9 @@ 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{}; extVisionSample _ev_sample_prev{}; RangeFinderConsistencyCheck _rng_consistency_check; @@ -348,7 +352,9 @@ protected: RingBuffer *_mag_buffer{nullptr}; RingBuffer *_baro_buffer{nullptr}; RingBuffer *_range_buffer{nullptr}; +#if defined(CONFIG_EKF2_AIRSPEED) RingBuffer *_airspeed_buffer{nullptr}; +#endif // CONFIG_EKF2_AIRSPEED RingBuffer *_flow_buffer{nullptr}; RingBuffer *_ext_vision_buffer{nullptr}; RingBuffer *_auxvel_buffer{nullptr}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a83a30cc99..1df244e3ea 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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_asp_delay(_params->airspeed_delay_ms), _param_ekf2_ev_delay(_params->ev_delay_ms), _param_ekf2_avel_delay(_params->auxvel_delay_ms), _param_ekf2_gyr_noise(_params->gyro_noise), @@ -86,12 +85,19 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt), _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), - _param_ekf2_head_noise(_params->mag_heading_noise), - _param_ekf2_mag_noise(_params->mag_noise), _param_ekf2_eas_noise(_params->eas_noise), + _param_ekf2_arsp_thr(_params->arsp_thr), +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) _param_ekf2_beta_gate(_params->beta_innov_gate), _param_ekf2_beta_noise(_params->beta_noise), + _param_ekf2_fuse_beta(_params->beta_fusion_enabled), +#endif // CONFIG_EKF2_SIDESLIP + _param_ekf2_head_noise(_params->mag_heading_noise), + _param_ekf2_mag_noise(_params->mag_noise), _param_ekf2_mag_decl(_params->mag_declination_deg), _param_ekf2_hdg_gate(_params->heading_innov_gate), _param_ekf2_mag_gate(_params->mag_innov_gate), @@ -151,8 +157,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _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_arsp_thr(_params->arsp_thr), - _param_ekf2_fuse_beta(_params->beta_fusion_enabled), _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), @@ -199,7 +203,9 @@ EKF2::~EKF2() perf_free(_ecl_ekf_update_full_perf); perf_free(_msg_missed_imu_perf); perf_free(_msg_missed_air_data_perf); +#if defined(CONFIG_EKF2_AIRSPEED) perf_free(_msg_missed_airspeed_perf); +#endif // CONFIG_EKF2_AIRSPEED perf_free(_msg_missed_distance_sensor_perf); perf_free(_msg_missed_gps_perf); perf_free(_msg_missed_landing_target_pose_perf); @@ -316,7 +322,9 @@ int EKF2::print_status() perf_print_counter(_ecl_ekf_update_full_perf); perf_print_counter(_msg_missed_imu_perf); perf_print_counter(_msg_missed_air_data_perf); +#if defined(CONFIG_EKF2_AIRSPEED) perf_print_counter(_msg_missed_airspeed_perf); +#endif // CONFIG_EKF2_AIRSPEED perf_print_counter(_msg_missed_distance_sensor_perf); perf_print_counter(_msg_missed_gps_perf); perf_print_counter(_msg_missed_landing_target_pose_perf); @@ -360,6 +368,7 @@ void EKF2::Run() _ekf.output_predictor().set_pos_correction_tc(_param_ekf2_tau_pos.get()); _ekf.output_predictor().set_vel_correction_tc(_param_ekf2_tau_vel.get()); +#if defined(CONFIG_EKF2_AIRSPEED) // The airspeed scale factor correcton is only available via parameter as used by the airspeed module param_t param_aspd_scale = param_find("ASPD_SCALE_1"); @@ -367,6 +376,8 @@ void EKF2::Run() param_get(param_aspd_scale, &_airspeed_scale_factor); } +#endif // CONFIG_EKF2_AIRSPEED + // if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output if (_params->baro_ctrl == 1) { float sens_baro_rate = 0.f; @@ -612,7 +623,9 @@ void EKF2::Run() .visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, }; +#if defined(CONFIG_EKF2_AIRSPEED) UpdateAirspeedSample(ekf2_timestamps); +#endif // CONFIG_EKF2_AIRSPEED UpdateAuxVelSample(ekf2_timestamps); UpdateBaroSample(ekf2_timestamps); UpdateExtVisionSample(ekf2_timestamps); @@ -801,12 +814,14 @@ void EKF2::VerifyParams() void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) { +#if defined(CONFIG_EKF2_AIRSPEED) // airspeed PublishAidSourceStatus(_ekf.aid_src_airspeed(), _status_airspeed_pub_last, _estimator_aid_src_airspeed_pub); - +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) // sideslip PublishAidSourceStatus(_ekf.aid_src_sideslip(), _status_sideslip_pub_last, _estimator_aid_src_sideslip_pub); - +#endif // CONFIG_EKF2_SIDESLIP // baro height PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); @@ -1131,8 +1146,12 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnov(innovations.drag); #endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_AIRSPEED) _ekf.getAirspeedInnov(innovations.airspeed); +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnov(innovations.beta); +#endif // CONFIG_EKF2_SIDESLIP _ekf.getHaglInnov(innovations.hagl); _ekf.getHaglRateInnov(innovations.hagl_rate); _ekf.getTerrainFlowInnov(innovations.terr_flow); @@ -1184,8 +1203,12 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovRatio(&test_ratios.drag[0]); #endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_AIRSPEED) _ekf.getAirspeedInnovRatio(test_ratios.airspeed); +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovRatio(test_ratios.beta); +#endif // CONFIG_EKF2_SIDESLIP _ekf.getHaglInnovRatio(test_ratios.hagl); _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); @@ -1213,8 +1236,12 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovVar(variances.drag); #endif // CONFIG_EKF2_DRAG_FUSION +#if defined(CONFIG_EKF2_AIRSPEED) _ekf.getAirspeedInnovVar(variances.airspeed); +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovVar(variances.beta); +#endif // CONFIG_EKF2_SIDESLIP _ekf.getHaglInnovVar(variances.hagl); _ekf.getHaglRateInnovVar(variances.hagl_rate); _ekf.getTerrainFlowInnovVar(variances.terr_flow); @@ -1622,10 +1649,15 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) const Vector2f wind_vel = _ekf.getWindVelocity(); const Vector2f wind_vel_var = _ekf.getWindVelocityVariance(); + +#if defined(CONFIG_EKF2_AIRSPEED) _ekf.getAirspeedInnov(wind.tas_innov); _ekf.getAirspeedInnovVar(wind.tas_innov_var); +#endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnov(wind.beta_innov); _ekf.getBetaInnovVar(wind.beta_innov_var); +#endif // CONFIG_EKF2_SIDESLIP wind.windspeed_north = wind_vel(0); wind.windspeed_east = wind_vel(1); @@ -1682,6 +1714,7 @@ float EKF2::filter_altitude_ellipsoid(float amsl_hgt) return amsl_hgt + _wgs84_hgt_offset; } +#if defined(CONFIG_EKF2_AIRSPEED) void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF airspeed sample @@ -1748,6 +1781,7 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } } } +#endif // CONFIG_EKF2_AIRSPEED void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index baa0906927..0750ab08fc 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -64,8 +64,6 @@ #include #include #include -#include -#include #include #include #include @@ -97,6 +95,11 @@ #include #include +#if defined(CONFIG_EKF2_AIRSPEED) +# include +# include +#endif // CONFIG_EKF2_AIRSPEED + extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem @@ -165,7 +168,9 @@ private: void PublishWindEstimate(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); +#if defined(CONFIG_EKF2_AIRSPEED) void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_AIRSPEED void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); @@ -227,8 +232,6 @@ private: perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; perf_counter_t _msg_missed_air_data_perf{nullptr}; - perf_counter_t _msg_missed_airspeed_perf{nullptr}; - perf_counter_t _msg_missed_airspeed_validated_perf{nullptr}; perf_counter_t _msg_missed_distance_sensor_perf{nullptr}; perf_counter_t _msg_missed_gps_perf{nullptr}; perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; @@ -265,9 +268,6 @@ private: hrt_abstime _last_sensor_bias_published{0}; hrt_abstime _last_gps_status_published{0}; - hrt_abstime _status_airspeed_pub_last{0}; - hrt_abstime _status_sideslip_pub_last{0}; - hrt_abstime _status_baro_hgt_pub_last{0}; hrt_abstime _status_rng_hgt_pub_last{0}; @@ -301,16 +301,30 @@ private: float _last_rng_hgt_bias_published{}; matrix::Vector3f _last_ev_bias_published{}; +#if defined(CONFIG_EKF2_AIRSPEED) + uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements hrt_abstime _airspeed_validated_timestamp_last{0}; + uORB::PublicationMulti _estimator_aid_src_airspeed_pub {ORB_ID(estimator_aid_src_airspeed)}; + hrt_abstime _status_airspeed_pub_last{0}; + + perf_counter_t _msg_missed_airspeed_perf{nullptr}; + perf_counter_t _msg_missed_airspeed_validated_perf{nullptr}; +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) + uORB::PublicationMulti _estimator_aid_src_sideslip_pub {ORB_ID(estimator_aid_src_sideslip)}; + hrt_abstime _status_sideslip_pub_last {0}; +#endif // CONFIG_EKF2_SIDESLIP + orb_advert_t _mavlink_log_pub{nullptr}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; - uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; @@ -361,10 +375,8 @@ private: uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationMulti _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)}; - uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; + uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; - uORB::PublicationMulti _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)}; 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)}; @@ -419,8 +431,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) _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) (ParamExtFloat) @@ -464,19 +474,35 @@ private: _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) (ParamExtFloat) - _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) + _param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) + + // control of airspeed fusion + (ParamExtFloat) + _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_SIDESLIP) + (ParamExtFloat) + _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) + (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) + + (ParamExtInt) + _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables +#endif // CONFIG_EKF2_SIDESLIP // control of magnetometer fusion (ParamExtFloat) _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) (ParamExtFloat) _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) - (ParamExtFloat) - _param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec) - (ParamExtFloat) - _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) - (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) + (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) (ParamExtFloat) _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) @@ -586,12 +612,6 @@ private: (ParamExtFloat) _param_ekf2_ev_pos_z, ///< Z position of VI sensor focal point in body frame (m) - // control of airspeed and sideslip fusion - (ParamExtFloat) - _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) - (ParamExtInt) - _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables - // output predictor filter time constants (ParamFloat) _param_ekf2_tau_vel, ///< time constant used by the output velocity complementary filter (sec) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 10c26e73e2..c764d2b83d 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -13,6 +13,14 @@ depends on MODULES_EKF2 ---help--- EKF2 support multiple instances and selector. +menuconfig EKF2_AIRSPEED +depends on MODULES_EKF2 + bool "airspeed fusion support" + default y + depends on EKF2_SIDESLIP + ---help--- + EKF2 airspeed fusion support. + menuconfig EKF2_BARO_COMPENSATION depends on MODULES_EKF2 bool "barometer compensation support" @@ -34,6 +42,13 @@ depends on MODULES_EKF2 ---help--- EKF2 GNSS yaw fusion support. +menuconfig EKF2_SIDESLIP +depends on MODULES_EKF2 + bool "sideslip fusion support" + default y + ---help--- + EKF2 sideslip fusion support. + menuconfig USER_EKF2 bool "ekf2 running as userspace module" default n