From fe0e3acf09366e00829c2511d49634830fef98b5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Mar 2023 09:40:00 -0400 Subject: [PATCH] ekf2: add kconfig option to enable/disable drag fusion --- boards/px4/fmu-v2/default.px4board | 2 +- src/modules/ekf2/CMakeLists.txt | 86 +++++++++++--------- src/modules/ekf2/EKF/CMakeLists.txt | 12 ++- src/modules/ekf2/EKF/common.h | 4 + src/modules/ekf2/EKF/control.cpp | 4 + src/modules/ekf2/EKF/ekf.h | 12 ++- src/modules/ekf2/EKF/estimator_interface.cpp | 8 ++ src/modules/ekf2/EKF/estimator_interface.h | 21 +++-- src/modules/ekf2/EKF2.cpp | 10 ++- src/modules/ekf2/EKF2.hpp | 4 +- src/modules/ekf2/Kconfig | 7 ++ 11 files changed, 115 insertions(+), 55 deletions(-) diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 5c414a109a..5f593c6267 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -23,13 +23,13 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_DRAG_FUSION is not set CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y CONFIG_MODULES_MC_ATT_CONTROL=y -CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 4bfd16eac5..2b8ad10114 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -66,6 +66,51 @@ add_subdirectory(Utility) # add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES}) # endif() +set(EKF_SRCS) +list(APPEND EKF_SRCS + EKF/airspeed_fusion.cpp + EKF/auxvel_fusion.cpp + EKF/baro_height_control.cpp + EKF/bias_estimator.cpp + EKF/control.cpp + EKF/covariance.cpp + EKF/ekf.cpp + EKF/ekf_helper.cpp + EKF/EKFGSF_yaw.cpp + EKF/estimator_interface.cpp + EKF/ev_control.cpp + EKF/ev_height_control.cpp + EKF/ev_pos_control.cpp + EKF/ev_vel_control.cpp + EKF/ev_yaw_control.cpp + EKF/fake_height_control.cpp + EKF/fake_pos_control.cpp + EKF/gnss_height_control.cpp + EKF/gps_checks.cpp + EKF/gps_control.cpp + EKF/gps_yaw_fusion.cpp + EKF/gravity_fusion.cpp + EKF/height_control.cpp + EKF/imu_down_sampler.cpp + EKF/mag_control.cpp + EKF/mag_fusion.cpp + EKF/optical_flow_control.cpp + EKF/optflow_fusion.cpp + EKF/output_predictor.cpp + 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_DRAG_FUSION) + list(APPEND EKF_SRCS EKF/drag_fusion.cpp) +endif() + px4_add_module( MODULE modules__ekf2 MAIN ekf2 @@ -81,44 +126,7 @@ px4_add_module( STACK_MAX 3600 SRCS - EKF/airspeed_fusion.cpp - EKF/auxvel_fusion.cpp - EKF/baro_height_control.cpp - EKF/bias_estimator.cpp - EKF/control.cpp - EKF/covariance.cpp - EKF/drag_fusion.cpp - EKF/ekf.cpp - EKF/ekf_helper.cpp - EKF/EKFGSF_yaw.cpp - EKF/estimator_interface.cpp - EKF/ev_control.cpp - EKF/ev_height_control.cpp - EKF/ev_pos_control.cpp - EKF/ev_vel_control.cpp - EKF/ev_yaw_control.cpp - EKF/fake_height_control.cpp - EKF/fake_pos_control.cpp - EKF/gnss_height_control.cpp - EKF/gps_checks.cpp - EKF/gps_control.cpp - EKF/gps_yaw_fusion.cpp - EKF/gravity_fusion.cpp - EKF/height_control.cpp - EKF/imu_down_sampler.cpp - EKF/mag_control.cpp - EKF/mag_fusion.cpp - EKF/optical_flow_control.cpp - EKF/optflow_fusion.cpp - EKF/output_predictor.cpp - 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 + ${EKF_SRCS} EKF2.cpp EKF2.hpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index fb5a2afbe2..162db69584 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -31,14 +31,14 @@ # ############################################################################ -add_library(ecl_EKF +set(EKF_SRCS) +list(APPEND EKF_SRCS airspeed_fusion.cpp auxvel_fusion.cpp baro_height_control.cpp bias_estimator.cpp control.cpp covariance.cpp - drag_fusion.cpp ekf.cpp ekf_helper.cpp EKFGSF_yaw.cpp @@ -72,6 +72,14 @@ add_library(ecl_EKF zero_velocity_update.cpp ) +if(CONFIG_EKF2_DRAG_FUSION) + list(APPEND EKF_SRCS drag_fusion.cpp) +endif() + +add_library(ecl_EKF + ${EKF_SRCS} +) + add_dependencies(ecl_EKF prebuild_targets) target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model) target_compile_options(ecl_EKF PRIVATE -fno-associative-math) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index c75ed2dae7..fdd74b2803 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -252,10 +252,12 @@ struct extVisionSample { int8_t quality{}; ///< quality indicator between 0 and 100 }; +#if defined(CONFIG_EKF2_DRAG_FUSION) struct dragSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) Vector2f accelXY{}; ///< measured specific force along the X and Y body axes (m/sec**2) }; +#endif // CONFIG_EKF2_DRAG_FUSION struct auxVelSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) @@ -446,11 +448,13 @@ struct parameters { // upper limit on airspeed used for correction (m/s**2) float max_correction_airspeed {20.0f}; +#if defined(CONFIG_EKF2_DRAG_FUSION) // multi-rotor drag specific force fusion float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 float bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2) float bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2) float mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s) +#endif // CONFIG_EKF2_DRAG_FUSION // control of accel error detection and mitigation (IMU clipping) const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 6d46c5f435..9573cd752c 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -108,7 +108,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlGpsFusion(imu_delayed); controlAirDataFusion(imu_delayed); controlBetaFusion(imu_delayed); + +#if defined(CONFIG_EKF2_DRAG_FUSION) controlDragFusion(); +#endif // CONFIG_EKF2_DRAG_FUSION + controlHeightFusion(imu_delayed); controlGravityFusion(imu_delayed); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 343405f7db..4590e8b776 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -171,9 +171,11 @@ public: void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); } void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); } +#if defined(CONFIG_EKF2_DRAG_FUSION) void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); } void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); } +#endif // CONFIG_EKF2_DRAG_FUSION 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; } @@ -537,8 +539,10 @@ private: Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance +#if defined(CONFIG_EKF2_DRAG_FUSION) Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) +#endif // CONFIG_EKF2_DRAG_FUSION float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) @@ -685,8 +689,13 @@ private: void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const; void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); +#if defined(CONFIG_EKF2_DRAG_FUSION) + // control fusion of multi-rotor drag specific force observations + void controlDragFusion(); + // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(const dragSample &drag_sample); +#endif // CONFIG_EKF2_DRAG_FUSION // fuse single velocity and position measurement bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); @@ -914,9 +923,6 @@ private: // control fusion of synthetic sideslip observations void controlBetaFusion(const imuSample &imu_delayed); - // control fusion of multi-rotor drag specific force observations - void controlDragFusion(); - // control fusion of fake position observations to constrain drift void controlFakePosFusion(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index ea5f66e38e..884cb95000 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -53,7 +53,9 @@ EstimatorInterface::~EstimatorInterface() delete _airspeed_buffer; delete _flow_buffer; delete _ext_vision_buffer; +#if defined(CONFIG_EKF2_DRAG_FUSION) delete _drag_buffer; +#endif // CONFIG_EKF2_DRAG_FUSION delete _auxvel_buffer; } @@ -85,7 +87,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _min_obs_interval_us = (imu_sample.time_us - _time_delayed_us) / (_obs_buffer_length - 1); } +#if defined(CONFIG_EKF2_DRAG_FUSION) setDragData(imu_sample); +#endif // CONFIG_EKF2_DRAG_FUSION } void EstimatorInterface::setMagData(const magSample &mag_sample) @@ -452,6 +456,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) } } +#if defined(CONFIG_EKF2_DRAG_FUSION) void EstimatorInterface::setDragData(const imuSample &imu) { // down-sample the drag specific force data by accumulating and calculating the mean when @@ -502,6 +507,7 @@ void EstimatorInterface::setDragData(const imuSample &imu) } } } +#endif // CONFIG_EKF2_DRAG_FUSION bool EstimatorInterface::initialise_interface(uint64_t timestamp) { @@ -682,9 +688,11 @@ void EstimatorInterface::print_status() printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size()); } +#if defined(CONFIG_EKF2_DRAG_FUSION) if (_drag_buffer) { printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size()); } +#endif // CONFIG_EKF2_DRAG_FUSION _output_predictor.print_status(); } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 7a08d3329b..ffade22958 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -294,8 +294,6 @@ protected: airspeedSample _airspeed_sample_delayed{}; flowSample _flow_sample_delayed{}; extVisionSample _ev_sample_prev{}; - dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) - RangeFinderConsistencyCheck _rng_consistency_check; float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) @@ -320,7 +318,13 @@ protected: AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio + +#if defined(CONFIG_EKF2_DRAG_FUSION) + RingBuffer *_drag_buffer{nullptr}; + dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio +#endif // CONFIG_EKF2_DRAG_FUSION + innovation_fault_status_u _innov_check_fail_status{}; bool _horizontal_deadreckon_time_exceeded{true}; @@ -345,7 +349,6 @@ protected: RingBuffer *_airspeed_buffer{nullptr}; RingBuffer *_flow_buffer{nullptr}; RingBuffer *_ext_vision_buffer{nullptr}; - RingBuffer *_drag_buffer{nullptr}; RingBuffer *_auxvel_buffer{nullptr}; RingBuffer *_system_flag_buffer{nullptr}; @@ -380,16 +383,18 @@ protected: private: - inline void setDragData(const imuSample &imu); +#if defined(CONFIG_EKF2_DRAG_FUSION) + void setDragData(const imuSample &imu); + + // Used by the multi-rotor specific drag force fusion + uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate + float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) +#endif // CONFIG_EKF2_DRAG_FUSION void printBufferAllocationFailed(const char *buffer_name); ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us}; unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) - - // Used by the multi-rotor specific drag force fusion - uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate - float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) }; #endif // !EKF_ESTIMATOR_INTERFACE_H diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b8cfe3b486..469cff3f39 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -161,10 +161,12 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim), _param_ekf2_abl_tau(_params->acc_bias_learn_tc), _param_ekf2_gyr_b_lim(_params->gyro_bias_lim), +#if defined(CONFIG_EKF2_DRAG_FUSION) _param_ekf2_drag_noise(_params->drag_noise), _param_ekf2_bcoef_x(_params->bcoef_x), _param_ekf2_bcoef_y(_params->bcoef_y), _param_ekf2_mcoef(_params->mcoef), +#endif // CONFIG_EKF2_DRAG_FUSION _param_ekf2_aspd_max(_params->max_correction_airspeed), _param_ekf2_pcoef_xp(_params->static_pressure_coef_xp), _param_ekf2_pcoef_xn(_params->static_pressure_coef_xn), @@ -1118,7 +1120,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _ekf.getFlowInnov(innovations.flow); _ekf.getHeadingInnov(innovations.heading); _ekf.getMagInnov(innovations.mag_field); +#if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnov(innovations.drag); +#endif // CONFIG_EKF2_DRAG_FUSION _ekf.getAirspeedInnov(innovations.airspeed); _ekf.getBetaInnov(innovations.beta); _ekf.getHaglInnov(innovations.hagl); @@ -1169,7 +1173,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) _ekf.getFlowInnovRatio(test_ratios.flow[0]); _ekf.getHeadingInnovRatio(test_ratios.heading); _ekf.getMagInnovRatio(test_ratios.mag_field[0]); +#if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovRatio(&test_ratios.drag[0]); +#endif // CONFIG_EKF2_DRAG_FUSION _ekf.getAirspeedInnovRatio(test_ratios.airspeed); _ekf.getBetaInnovRatio(test_ratios.beta); _ekf.getHaglInnovRatio(test_ratios.hagl); @@ -1196,7 +1202,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) _ekf.getFlowInnovVar(variances.flow); _ekf.getHeadingInnovVar(variances.heading); _ekf.getMagInnovVar(variances.mag_field); +#if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovVar(variances.drag); +#endif // CONFIG_EKF2_DRAG_FUSION _ekf.getAirspeedInnovVar(variances.airspeed); _ekf.getBetaInnovVar(variances.beta); _ekf.getHaglInnovVar(variances.hagl); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 47572e0107..94588cccf4 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -613,12 +613,14 @@ private: (ParamExtFloat) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s) +#if defined(CONFIG_EKF2_DRAG_FUSION) // Multi-rotor drag specific force fusion (ParamExtFloat) _param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2 (ParamExtFloat) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2) (ParamExtFloat) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2) (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) +#endif // CONFIG_EKF2_DRAG_FUSION // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index a1d1e9be45..b869050945 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -13,6 +13,13 @@ depends on MODULES_EKF2 ---help--- EKF2 support multiple instances and selector. +menuconfig EKF2_DRAG_FUSION +depends on MODULES_EKF2 + bool "drag fusion support" + default y + ---help--- + EKF2 drag fusion support. + menuconfig USER_EKF2 bool "ekf2 running as userspace module" default n