From 0784901a667ac74c7d99178a39f4c649e617baaf Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Mar 2023 16:51:49 -0400 Subject: [PATCH] ekf2: add kconfig option to enable/disable optical flow fusion --- src/modules/ekf2/CMakeLists.txt | 9 +++- src/modules/ekf2/EKF/CMakeLists.txt | 10 ++-- src/modules/ekf2/EKF/common.h | 5 +- src/modules/ekf2/EKF/control.cpp | 4 ++ src/modules/ekf2/EKF/ekf.cpp | 2 + src/modules/ekf2/EKF/ekf.h | 24 +++++---- src/modules/ekf2/EKF/ekf_helper.cpp | 16 +++++- src/modules/ekf2/EKF/estimator_interface.cpp | 8 +++ src/modules/ekf2/EKF/estimator_interface.h | 27 ++++++---- src/modules/ekf2/EKF/gps_control.cpp | 16 ++++-- src/modules/ekf2/EKF/terrain_estimator.cpp | 7 +++ src/modules/ekf2/EKF2.cpp | 40 +++++++++++--- src/modules/ekf2/EKF2.hpp | 53 ++++++++++++------- src/modules/ekf2/Kconfig | 7 +++ src/modules/ekf2/Utility/PreFlightChecker.cpp | 9 +++- src/modules/ekf2/Utility/PreFlightChecker.hpp | 23 +++++--- 16 files changed, 194 insertions(+), 66 deletions(-) diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 7c8c37414c..a81f15efb3 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -86,8 +86,6 @@ list(APPEND EKF_SRCS 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 @@ -124,6 +122,13 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_OPTICAL_FLOW) + list(APPEND EKF_SRCS + EKF/optical_flow_control.cpp + EKF/optflow_fusion.cpp + ) +endif() + if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 7cae087bca..e804d5b866 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -51,8 +51,6 @@ list(APPEND EKF_SRCS imu_down_sampler.cpp mag_control.cpp mag_fusion.cpp - optical_flow_control.cpp - optflow_fusion.cpp output_predictor.cpp range_finder_consistency_check.cpp range_height_control.cpp @@ -85,11 +83,17 @@ if(CONFIG_EKF2_EXTERNAL_VISION) ) endif() - if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_fusion.cpp) endif() +if(CONFIG_EKF2_OPTICAL_FLOW) + list(APPEND EKF_SRCS + optical_flow_control.cpp + optflow_fusion.cpp + ) +endif() + if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS sideslip_fusion.cpp) endif() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ffecad5e2d..d6113bb4bf 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -310,7 +310,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 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) // input noise @@ -412,11 +411,15 @@ struct parameters { // gravity fusion float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + 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 + // optical flow fusion float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) +#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 diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 04790bae63..20ff1dcde3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -104,7 +104,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) // control use of observations for aiding controlMagFusion(); + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) controlOpticalFlowFusion(imu_delayed); +#endif // CONFIG_EKF2_OPTICAL_FLOW + controlGpsFusion(imu_delayed); #if defined(CONFIG_EKF2_AIRSPEED) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 530df6dddf..a929408274 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -148,8 +148,10 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_aux_vel); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) resetEstimatorAidStatus(_aid_src_optical_flow); resetEstimatorAidStatus(_aid_src_terrain_optical_flow); +#endif // CONFIG_EKF2_OPTICAL_FLOW } bool Ekf::update() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 340d75aeb2..1b6f2e688b 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -105,6 +105,7 @@ public: void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); } #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) void getFlowInnov(float flow_innov[2]) const; void getFlowInnovVar(float flow_innov_var[2]) const; void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); } @@ -122,6 +123,10 @@ public: void getTerrainFlowInnovVar(float flow_innov_var[2]) const; void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } + const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } + const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } +#endif // CONFIG_EKF2_OPTICAL_FLOW + void getHeadingInnov(float &heading_innov) const { if (_control_status.flags.mag_hdg) { @@ -494,9 +499,6 @@ public: const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL - const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } - const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } - private: // set the internal states and status to their default value @@ -645,8 +647,10 @@ private: estimator_aid_source2d_s _aid_src_aux_vel{}; #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) estimator_aid_source2d_s _aid_src_optical_flow{}; estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; +#endif // CONFIG_EKF2_OPTICAL_FLOW // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) @@ -809,12 +813,6 @@ private: void resetVerticalVelocityToZero(); - // fuse optical flow line of sight rate measurements - void updateOptFlow(estimator_aid_source2d_s &aid_src); - void fuseOptFlow(); - float predictFlowRange(); - Vector2f predictFlowVelBody(); - // horizontal and vertical position aid source void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const; @@ -850,6 +848,7 @@ private: void stopHaglRngFusion(); float getRngVar(); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) // update the terrain vertical position estimate using an optical flow measurement void controlHaglFlowFusion(); void startHaglFlowFusion(); @@ -857,6 +856,13 @@ private: void stopHaglFlowFusion(); void fuseFlowForTerrain(estimator_aid_source2d_s &flow); + // fuse optical flow line of sight rate measurements + void updateOptFlow(estimator_aid_source2d_s &aid_src); + void fuseOptFlow(); + float predictFlowRange(); + Vector2f predictFlowVelBody(); +#endif // CONFIG_EKF2_OPTICAL_FLOW + void controlHaglFakeFusion(); // reset the heading and magnetic field states using the declination and magnetometer measurements diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 2790c614a3..a36918d325 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -355,6 +355,7 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const } #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) void Ekf::getFlowInnov(float flow_innov[2]) const { flow_innov[0] = _aid_src_optical_flow.innovation[0]; @@ -378,6 +379,7 @@ void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; } +#endif // CONFIG_EKF2_OPTICAL_FLOW // get the state vector at the delayed time horizon matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const @@ -519,10 +521,12 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const if (_horizontal_deadreckon_time_exceeded) { float vel_err_conservative = 0.0f; +#if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_control_status.flags.opt_flow) { float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm(); } +#endif // CONFIG_EKF2_OPTICAL_FLOW if (_control_status.flags.gps) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); @@ -568,7 +572,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); - const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); // Keep within range sensor limit when using rangefinder as primary height source if (relying_on_rangefinder) { @@ -576,7 +579,10 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_max = rangefinder_hagl_max; } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) // Keep within flow AND range sensor limits when exclusively using optical flow + const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); + if (relying_on_optical_flow) { // Calculate optical flow limits const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance); @@ -591,6 +597,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = flow_hagl_min; *hagl_max = flow_hagl_max; } +#endif // CONFIG_EKF2_OPTICAL_FLOW } void Ekf::resetImuBias() @@ -680,10 +687,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f } #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_OPTICAL_FLOW) if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max()); vel = math::max(of_vel, FLT_MIN); } +#endif // CONFIG_EKF2_OPTICAL_FLOW // return the combined vertical position innovation test ratio float hgt_sum = 0.f; @@ -802,7 +811,10 @@ void Ekf::updateHorizontalDeadReckoningstatus() && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)); - const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max); + bool optFlowAiding = false; +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max); +#endif // CONFIG_EKF2_OPTICAL_FLOW bool airDataAiding = false; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 47fa234c09..7d2cf3a4ee 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -53,7 +53,9 @@ EstimatorInterface::~EstimatorInterface() #if defined(CONFIG_EKF2_AIRSPEED) delete _airspeed_buffer; #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_OPTICAL_FLOW) delete _flow_buffer; +#endif // _flow_buffer #if defined(CONFIG_EKF2_EXTERNAL_VISION) delete _ext_vision_buffer; #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -322,6 +324,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) } } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) void EstimatorInterface::setOpticalFlowData(const flowSample &flow) { if (!_initialised) { @@ -356,6 +359,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) @@ -560,9 +564,11 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms); } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) { max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); } +#endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_params.ev_ctrl > 0) { @@ -704,9 +710,11 @@ void EstimatorInterface::print_status() } #endif // CONFIG_EKF2_AIRSPEED +#if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_flow_buffer) { printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size()); } +#endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_ext_vision_buffer) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1a1606d668..5513b3c695 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -96,9 +96,19 @@ public: void setRangeData(const rangeSample &range_sample); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) // if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead void setOpticalFlowData(const flowSample &flow); + // set sensor limitations reported by the optical flow sensor + void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance) + { + _flow_max_rate = max_flow_rate; + _flow_min_distance = min_distance; + _flow_max_distance = max_distance; + } +#endif // CONFIG_EKF2_OPTICAL_FLOW + #if defined(CONFIG_EKF2_EXTERNAL_VISION) // set external vision position and attitude data void setExtVisionData(const extVisionSample &evdata); @@ -159,14 +169,6 @@ public: _range_sensor.setLimits(min_distance, max_distance); } - // set sensor limitations reported by the optical flow sensor - void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance) - { - _flow_max_rate = max_flow_rate; - _flow_min_distance = min_distance; - _flow_max_distance = max_distance; - } - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; @@ -302,8 +304,6 @@ protected: airspeedSample _airspeed_sample_delayed{}; #endif // CONFIG_EKF2_AIRSPEED - flowSample _flow_sample_delayed{}; - #if defined(CONFIG_EKF2_EXTERNAL_VISION) extVisionSample _ev_sample_prev{}; #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -312,10 +312,16 @@ protected: float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + RingBuffer *_flow_buffer{nullptr}; + + flowSample _flow_sample_delayed{}; + // Sensor limitations float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m) float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m) +#endif // CONFIG_EKF2_OPTICAL_FLOW 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 @@ -365,7 +371,6 @@ protected: #if defined(CONFIG_EKF2_AIRSPEED) RingBuffer *_airspeed_buffer{nullptr}; #endif // CONFIG_EKF2_AIRSPEED - RingBuffer *_flow_buffer{nullptr}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) RingBuffer *_ext_vision_buffer{nullptr}; diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index b33627eb25..9848ad6b0b 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -274,9 +274,19 @@ bool Ekf::shouldResetGpsFusion() const /* We are relying on aiding to constrain drift so after a specified time * with no aiding we need to do something */ - const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - && isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); + bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (has_horizontal_aiding_timed_out) { + // horizontal aiding hasn't timed out if optical flow still active + if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) { + has_horizontal_aiding_timed_out = false; + } + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW const bool is_reset_required = has_horizontal_aiding_timed_out || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 56900214fe..bd07cce0ef 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -45,7 +45,10 @@ void Ekf::initHagl() { +#if defined(CONFIG_EKF2_OPTICAL_FLOW) stopHaglFlowFusion(); +#endif // CONFIG_EKF2_OPTICAL_FLOW + stopHaglRngFusion(); // assume a ground clearance @@ -66,7 +69,9 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed) predictHagl(imu_delayed); controlHaglRngFusion(); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) controlHaglFlowFusion(); +#endif // CONFIG_EKF2_OPTICAL_FLOW controlHaglFakeFusion(); // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) @@ -246,6 +251,7 @@ void Ekf::fuseHaglRng() } } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) void Ekf::controlHaglFlowFusion() { if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) { @@ -392,6 +398,7 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) //_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain? _aid_src_optical_flow.fused = true; } +#endif // CONFIG_EKF2_OPTICAL_FLOW void Ekf::controlHaglFakeFusion() { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bdb22797e6..3f06df9ff5 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -63,7 +63,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_delay(_params->mag_delay_ms), _param_ekf2_baro_delay(_params->baro_delay_ms), _param_ekf2_gps_delay(_params->gps_delay_ms), - _param_ekf2_of_delay(_params->flow_delay_ms), _param_ekf2_rng_delay(_params->range_delay_ms), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), @@ -145,10 +144,16 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _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_delay(_params->flow_delay_ms), _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), _param_ekf2_of_qmin(_params->flow_qual_min), _param_ekf2_of_gate(_params->flow_innov_gate), + _param_ekf2_of_pos_x(_params->flow_pos_body(0)), + _param_ekf2_of_pos_y(_params->flow_pos_body(1)), + _param_ekf2_of_pos_z(_params->flow_pos_body(2)), +#endif // CONFIG_EKF2_OPTICAL_FLOW _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)), @@ -158,9 +163,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), - _param_ekf2_of_pos_x(_params->flow_pos_body(0)), - _param_ekf2_of_pos_y(_params->flow_pos_body(1)), - _param_ekf2_of_pos_z(_params->flow_pos_body(2)), _param_ekf2_gbias_init(_params->switch_on_gyro_bias), _param_ekf2_abias_init(_params->switch_on_accel_bias), _param_ekf2_angerr_init(_params->initial_tilt_err), @@ -217,7 +219,9 @@ EKF2::~EKF2() #endif // CONFIG_EKF2_AUXVEL perf_free(_msg_missed_magnetometer_perf); perf_free(_msg_missed_odometry_perf); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_free(_msg_missed_optical_flow_perf); +#endif // CONFIG_EKF2_OPTICAL_FLOW } #if defined(CONFIG_EKF2_MULTI_INSTANCE) @@ -229,7 +233,9 @@ bool EKF2::multi_init(int imu, int mag) _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) _estimator_optical_flow_vel_pub.advertise(); +#endif // CONFIG_EKF2_OPTICAL_FLOW _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); _estimator_status_flags_pub.advertise(); @@ -342,7 +348,9 @@ int EKF2::print_status() #endif // CONFIG_EKF2_AUXVEL perf_print_counter(_msg_missed_magnetometer_perf); perf_print_counter(_msg_missed_odometry_perf); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_print_counter(_msg_missed_optical_flow_perf); +#endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(DEBUG_BUILD) _ekf.print_status(); @@ -645,7 +653,9 @@ void EKF2::Run() #if defined(CONFIG_EKF2_EXTERNAL_VISION) UpdateExtVisionSample(ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_OPTICAL_FLOW) UpdateFlowSample(ekf2_timestamps); +#endif // CONFIG_EKF2_OPTICAL_FLOW UpdateGpsSample(ekf2_timestamps); UpdateMagSample(ekf2_timestamps); UpdateRangeSample(ekf2_timestamps); @@ -674,7 +684,9 @@ void EKF2::Run() 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); @@ -883,10 +895,12 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub); PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last, _estimator_aid_src_terrain_optical_flow_pub); +#endif // CONFIG_EKF2_OPTICAL_FLOW } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -1171,7 +1185,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnov(innovations.aux_hvel); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnov(innovations.flow); + _ekf.getTerrainFlowInnov(innovations.terr_flow); +#endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnov(innovations.heading); _ekf.getMagInnov(innovations.mag_field); #if defined(CONFIG_EKF2_DRAG_FUSION) @@ -1185,7 +1202,6 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #endif // CONFIG_EKF2_SIDESLIP _ekf.getHaglInnov(innovations.hagl); _ekf.getHaglRateInnov(innovations.hagl_rate); - _ekf.getTerrainFlowInnov(innovations.terr_flow); _ekf.getGravityInnov(innovations.gravity); // Not yet supported innovations.aux_vvel = NAN; @@ -1202,7 +1218,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) if (!_ekf.control_status_flags().in_air) { // TODO: move to run before publications _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); +#endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); @@ -1235,7 +1253,10 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnovRatio(test_ratios.flow[0]); + _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); +#endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovRatio(test_ratios.heading); _ekf.getMagInnovRatio(test_ratios.mag_field[0]); #if defined(CONFIG_EKF2_DRAG_FUSION) @@ -1249,7 +1270,6 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #endif // CONFIG_EKF2_SIDESLIP _ekf.getHaglInnovRatio(test_ratios.hagl); _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); - _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); _ekf.getGravityInnovRatio(test_ratios.gravity[0]); // Not yet supported test_ratios.aux_vvel = NAN; @@ -1272,7 +1292,10 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovVar(variances.aux_hvel); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnovVar(variances.flow); + _ekf.getTerrainFlowInnovVar(variances.terr_flow); +#endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovVar(variances.heading); _ekf.getMagInnovVar(variances.mag_field); #if defined(CONFIG_EKF2_DRAG_FUSION) @@ -1286,7 +1309,6 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #endif // CONFIG_EKF2_SIDESLIP _ekf.getHaglInnovVar(variances.hagl); _ekf.getHaglRateInnovVar(variances.hagl_rate); - _ekf.getTerrainFlowInnovVar(variances.terr_flow); _ekf.getGravityInnovVar(variances.gravity); // Not yet supported variances.aux_vvel = NAN; @@ -1711,6 +1733,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) { const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; @@ -1734,6 +1757,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _estimator_optical_flow_vel_pub.publish(flow_vel); } } +#endif // CONFIG_EKF2_OPTICAL_FLOW float EKF2::filter_altitude_ellipsoid(float amsl_hgt) { @@ -2055,6 +2079,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) } #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_OPTICAL_FLOW) bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF flow sample @@ -2113,6 +2138,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) return new_optical_flow; } +#endif // CONFIG_EKF2_OPTICAL_FLOW void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index f0611de962..af7a5b632e 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -88,8 +88,6 @@ #include #include #include -#include -#include #include #include #include @@ -103,6 +101,11 @@ # include #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) +# include +# include +#endif // CONFIG_EKF2_OPTICAL_FLOW + extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem @@ -166,7 +169,9 @@ private: void PublishLocalPosition(const hrt_abstime ×tamp); void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample); void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom); +#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); @@ -181,8 +186,12 @@ private: void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_AUXVEL void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); +#if defined(CONFIG_EKF2_EXTERNAL_VISION) bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_OPTICAL_FLOW) bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_OPTICAL_FLOW void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); @@ -244,7 +253,6 @@ private: perf_counter_t _msg_missed_gps_perf{nullptr}; perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; - perf_counter_t _msg_missed_optical_flow_perf{nullptr}; // Used to control saving of mag declination to be used on next startup bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved @@ -319,8 +327,16 @@ private: perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)}; + uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; + uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; + uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; + hrt_abstime _status_optical_flow_pub_last{0}; hrt_abstime _status_terrain_optical_flow_pub_last{0}; + perf_counter_t _msg_missed_optical_flow_perf{nullptr}; +#endif // CONFIG_EKF2_OPTICAL_FLOW float _last_baro_bias_published{}; float _last_gnss_hgt_bias_published{}; @@ -356,7 +372,6 @@ private: 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::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; @@ -394,7 +409,6 @@ 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 _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_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; @@ -415,9 +429,6 @@ private: uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; - uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; - uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; - // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; uORB::PublicationMulti _local_position_pub; @@ -442,8 +453,6 @@ private: _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _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) @@ -606,15 +615,25 @@ private: (ParamExtFloat) _param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2) +#if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow fusion + (ParamExtFloat) + _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_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) + _param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtFloat) - _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) + _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtInt) - _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor (ParamExtFloat) - _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) + _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) + (ParamExtFloat) + _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) +#endif // CONFIG_EKF2_OPTICAL_FLOW // sensor positions in body frame (ParamExtFloat) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m) @@ -626,12 +645,6 @@ private: (ParamExtFloat) _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) (ParamExtFloat) _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) (ParamExtFloat) _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m) - (ParamExtFloat) - _param_ekf2_of_pos_z, ///< Z position of optical flow sensor focal point in body frame (m) // output predictor filter time constants (ParamFloat) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index e26ce74307..543fa7f7f2 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -56,6 +56,13 @@ depends on MODULES_EKF2 ---help--- EKF2 GNSS yaw fusion support. +menuconfig EKF2_OPTICAL_FLOW +depends on MODULES_EKF2 + bool "optical flow fusion support" + default y + ---help--- + EKF2 optical flow fusion support. + menuconfig EKF2_SIDESLIP depends on MODULES_EKF2 bool "sideslip fusion support" diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index e490018d7d..fa80e5336b 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -82,6 +82,8 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_ has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim); } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (_is_using_flow_aiding) { const Vector2f flow_innov = Vector2f(innov.flow); Vector2f flow_innov_lpf; @@ -90,6 +92,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_ has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim); } +#endif // CONFIG_EKF2_OPTICAL_FLOW return has_failed; } @@ -143,7 +146,6 @@ bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vecto void PreFlightChecker::reset() { _is_using_gps_aiding = false; - _is_using_flow_aiding = false; _is_using_ev_pos_aiding = false; _is_using_ev_vel_aiding = false; _is_using_baro_hgt_aiding = false; @@ -162,6 +164,11 @@ void PreFlightChecker::reset() _filter_rng_hgt_innov.reset(); _filter_ev_hgt_innov.reset(); _filter_heading_innov.reset(); + + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + _is_using_flow_aiding = false; _filter_flow_x_innov.reset(); _filter_flow_y_innov.reset(); +#endif // CONFIG_EKF2_OPTICAL_FLOW } diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp index a3b4fae078..6dfd8b55c0 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -75,7 +75,9 @@ public: void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; } void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; } +#if defined(CONFIG_EKF2_OPTICAL_FLOW) void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } +#endif // CONFIG_EKF2_OPTICAL_FLOW void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; } void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; } @@ -150,7 +152,6 @@ private: bool _can_observe_heading_in_flight{}; bool _is_using_gps_aiding{}; - bool _is_using_flow_aiding{}; bool _is_using_ev_pos_aiding{}; bool _is_using_ev_vel_aiding{}; @@ -164,8 +165,6 @@ private: InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec) InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec) InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad) - InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad) - InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad) // Preflight low pass filter height innovation (m) InnovationLpf _filter_baro_hgt_innov; @@ -173,6 +172,18 @@ private: InnovationLpf _filter_rng_hgt_innov; InnovationLpf _filter_ev_hgt_innov; +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + bool _is_using_flow_aiding {}; + InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad) + InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad) + + // Maximum permissible flow innovation to pass pre-flight checks + static constexpr float _flow_innov_test_lim = 0.5f; + + // Preflight flow innovation spike limit (rad) + static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim; +#endif // CONFIG_EKF2_OPTICAL_FLOW + // Preflight low pass filter time constant inverse (1/sec) static constexpr float _innov_lpf_tau_inv = 0.2f; // Maximum permissible velocity innovation to pass pre-flight checks (m/sec) @@ -183,13 +194,11 @@ private: static constexpr float _nav_heading_innov_test_lim = 0.25f; // Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad) static constexpr float _heading_innov_test_lim = 0.52f; - // Maximum permissible flow innovation to pass pre-flight checks - static constexpr float _flow_innov_test_lim = 0.5f; + // Preflight velocity innovation spike limit (m/sec) static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; // Preflight position innovation spike limit (m) static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; - // Preflight flow innovation spike limit (rad) - static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim; + }; #endif // !EKF2_PREFLIGHTCHECKER_HPP