diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 497eb586a9..97f7c32fe3 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -58,7 +58,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6); - if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_fc.drag.intended == 0))) { + if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.ekf2_drag_ctrl == 0))) { _control_status.flags.wind = false; } @@ -82,7 +82,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) #endif // CONFIG_EKF2_GNSS - if (_params.ekf2_arsp_thr <= 0.f) { + _fc.aspd.available = (_params.ekf2_arsp_thr > 0.f); + + if (!_fc.aspd.intended()) { stopAirspeedFusion(); return; } diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index ec3525c091..9ff7d45993 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -132,6 +132,19 @@ bool AuxGlobalPosition::anySourceFusing() const return false; } +uint8_t AuxGlobalPosition::sourceFusingBitmask() const +{ + uint8_t mask = 0; + + for (int i = 0; i < MAX_AGP_IDS; i++) { + if (_sources[i] && _sources[i]->isFusing()) { + mask |= (1u << i); + } + } + + return mask; +} + int32_t AuxGlobalPosition::getAgpParamInt32(const char *param_suffix, int instance) const { char param_name[20] {}; diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index 4b30f5d9d7..fb1d1421b6 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp @@ -67,6 +67,7 @@ public: */ float testRatioFiltered() const; bool anySourceFusing() const; + uint8_t sourceFusingBitmask() const; int32_t getIdParam(int instance); void setIdParam(int instance, int32_t sensor_id); int mapSensorIdToSlot(int32_t sensor_id); diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp index b04df2ee83..0487691dd8 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position_control.cpp @@ -99,11 +99,13 @@ void AgpSource::bufferData(const aux_global_position_s &msg, const estimator::im bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { + ekf._fc.agp[_slot].available = (_params.ctrl != 0); + AuxGlobalPositionSample sample; if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { - if (!(ekf._fc.agp[_slot].intended & static_cast(Ctrl::kHPos))) { + if (!ekf._fc.agp[_slot].intended()) { return true; } diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 7dce041475..cf623bc7b6 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -40,6 +40,8 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) { + _fc.baro.available = (_params.ekf2_baro_ctrl != 0); + static constexpr const char *HGT_SRC_NAME = "baro"; auto &aid_src = _aid_src_baro_hgt; @@ -111,7 +113,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) } // determine if we should use height aiding - const bool continuing_conditions_passing = (_fc.baro.intended == 1) + const bool continuing_conditions_passing = _fc.baro.intended() && measurement_valid && (_baro_counter > _obs_buffer_length) && !_control_status.flags.baro_fault; diff --git a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp index 9ed7b7ae67..de496a9fab 100644 --- a/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp @@ -45,7 +45,7 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed) { - if ((_fc.drag.intended > 0) && _drag_buffer) { + if ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) { if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) { _control_status.flags.wind = true; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 8f93f01b76..3d89944c7b 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -41,6 +41,8 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) { + _fc.ev.available = (_params.ekf2_ev_ctrl != 0); + _ev_pos_b_est.predict(_dt_ekf_avg); _ev_hgt_b_est.predict(_dt_ekf_avg); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 527ec5014f..6d2067423d 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -102,7 +102,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } - const bool continuing_conditions_passing = (_fc.ev.intended & static_cast(EvCtrl::VPOS)) + const bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast(EvCtrl::VPOS)) && measurement_valid; const bool starting_conditions_passing = common_starting_conditions_passing diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index d07d31f894..d4e6b875ab 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -49,7 +49,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); // determine if we should use EV position aiding - bool continuing_conditions_passing = (_fc.ev.intended & static_cast(EvCtrl::HPOS)) + bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast(EvCtrl::HPOS)) && _control_status.flags.tilt_align && PX4_ISFINITE(ev_sample.pos(0)) && PX4_ISFINITE(ev_sample.pos(1)); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index 0989af3956..31b82dc357 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); // determine if we should use EV velocity aiding - bool continuing_conditions_passing = (_fc.ev.intended & static_cast(EvCtrl::VEL)) + bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast(EvCtrl::VEL)) && _control_status.flags.tilt_align && ev._sample.vel.isAllFinite() && !ev._sample.vel.longerThan(_params.ekf2_vel_lim); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp index 0d419ad741..4e588e192b 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp @@ -66,7 +66,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample } // determine if we should use EV yaw aiding - bool continuing_conditions_passing = (_fc.ev.intended & static_cast(EvCtrl::YAW)) + bool continuing_conditions_passing = _fc.ev.enabled && (_params.ekf2_ev_ctrl & static_cast(EvCtrl::YAW)) && _control_status.flags.tilt_align && !_control_status.flags.ev_yaw_fault && PX4_ISFINITE(aid_src.observation) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index 5c923abe42..ccfd3d3d5a 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -82,7 +82,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) && _gnss_checks.passed() && !_control_status.flags.gnss_fault; - const bool continuing_conditions_passing = (_fc.gps.intended & static_cast(GnssCtrl::VPOS)) + const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::VPOS)) && common_conditions_passing; const bool starting_conditions_passing = continuing_conditions_passing diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index 8233eee1a1..709fb76211 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -47,7 +47,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) { - if (!(_fc.gps.intended & static_cast(GnssCtrl::YAW)) + if (!(_params.ekf2_gps_ctrl & static_cast(GnssCtrl::YAW)) || _control_status.flags.gnss_yaw_fault) { stopGnssYawFusion(); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index c8ff42f4ae..b9d0df0a5d 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -41,7 +41,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) { - if (!_gps_buffer || (_fc.gps.intended == 0)) { + _fc.gps.available = (_params.ekf2_gps_ctrl != 0); + + if (!_gps_buffer || !_fc.gps.intended()) { stopGnssFusion(); return; } @@ -126,7 +128,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset) { - const bool continuing_conditions_passing = (_fc.gps.intended & static_cast(GnssCtrl::VEL)) + const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::VEL)) && _control_status.flags.tilt_align && _control_status.flags.yaw_align && !_control_status.flags.gnss_fault @@ -182,7 +184,7 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset) { - const bool gnss_pos_enabled = (_fc.gps.intended & static_cast(GnssCtrl::HPOS)); + const bool gnss_pos_enabled = (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::HPOS)); const bool continuing_conditions_passing = gnss_pos_enabled && _control_status.flags.tilt_align @@ -385,8 +387,8 @@ void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel) _yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air); // Try to align yaw using estimate if available - if (((_fc.gps.intended & static_cast(GnssCtrl::VEL)) - || (_fc.gps.intended & static_cast(GnssCtrl::HPOS))) + if (((_params.ekf2_gps_ctrl & static_cast(GnssCtrl::VEL)) + || (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::HPOS))) && !_control_status.flags.yaw_align && _control_status.flags.tilt_align) { if (resetYawToEKFGSF()) { diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index 6c4270ba98..fb0b3bc932 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -59,7 +59,7 @@ void Ekf::controlGravityFusion(const imuSample &imu) && (accel_lpf_norm_sq < sq(upper_accel_limit)); // fuse gravity observation if our overall acceleration isn't too big - _control_status.flags.gravity_vector = (_fc.imu.intended & static_cast(ImuCtrl::GravityVector)) + _control_status.flags.gravity_vector = (_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GravityVector)) && (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest) && !isHorizontalAidingActive() && _control_status.flags.tilt_align; // Let fake position do the initial alignment (more robust before takeoff) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 7069190a82..c3a6d9eae6 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -54,7 +54,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) _control_status.flags.mag_aligned_in_flight = false; } - if (_fc.mag.intended == MagFuseType::NONE) { + _fc.mag.available = _params.ekf2_mag_type != static_cast(MagFuseType::NONE); + + if (!_fc.mag.intended()) { stopMagFusion(); return; } @@ -150,9 +152,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) math::max(_params.ekf2_mag_gate, 1.f)); // innovation gate // determine if we should use mag fusion - bool continuing_conditions_passing = ((_fc.mag.intended == MagFuseType::INIT) - || (_fc.mag.intended == MagFuseType::AUTO) - || (_fc.mag.intended == MagFuseType::HEADING)) + bool continuing_conditions_passing = ((_params.ekf2_mag_type == static_cast(MagFuseType::INIT)) + || (_params.ekf2_mag_type == static_cast(MagFuseType::AUTO)) + || (_params.ekf2_mag_type == static_cast(MagFuseType::HEADING))) && _control_status.flags.tilt_align && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) && mag_sample.mag.longerThan(0.f) @@ -187,12 +189,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) && (!_control_status.flags.yaw_manual || _control_status.flags.mag_aligned_in_flight); _control_status.flags.mag_3D = common_conditions_passing - && (_fc.mag.intended == MagFuseType::AUTO) + && (_params.ekf2_mag_type == static_cast(MagFuseType::AUTO)) && _control_status.flags.mag_aligned_in_flight; _control_status.flags.mag_hdg = common_conditions_passing - && ((_fc.mag.intended == MagFuseType::HEADING) - || (_fc.mag.intended == MagFuseType::AUTO && !_control_status.flags.mag_3D)); + && ((_params.ekf2_mag_type == static_cast(MagFuseType::HEADING)) + || (_params.ekf2_mag_type == static_cast(MagFuseType::AUTO) && !_control_status.flags.mag_3D)); } if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index fd31089a14..bfbc50c4d8 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -42,7 +42,9 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) { - if (!_flow_buffer || (_fc.of.intended != 1)) { + _fc.of.available = (_params.ekf2_of_ctrl != 0); + + if (!_flow_buffer || !_fc.of.intended()) { stopFlowFusion(); return; } @@ -146,7 +148,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) && !flow_sample.flow_rate.longerThan(_flow_max_rate) && !flow_compensated.longerThan(_flow_max_rate); - const bool continuing_conditions_passing = (_fc.of.intended == 1) + const bool continuing_conditions_passing = _fc.of.intended() && _control_status.flags.tilt_align && is_within_sensor_dist; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 61fe5a95d2..7efcb9bebe 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -42,6 +42,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) { + _fc.rng.available = (_params.ekf2_rng_ctrl != static_cast(RngCtrl::DISABLED)); + static constexpr const char *HGT_SRC_NAME = "RNG"; bool rng_data_ready = false; @@ -127,8 +129,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) aid_src.innovation_rejected = false; } - const bool continuing_conditions_passing = ((_fc.rng.intended == static_cast(RngCtrl::ENABLED)) - || (_fc.rng.intended == static_cast(RngCtrl::CONDITIONAL))) + const bool continuing_conditions_passing = _fc.rng.intended() && _control_status.flags.tilt_align && measurement_valid; @@ -138,11 +139,11 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && _range_sensor.isDataHealthy(); const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) - && (_fc.rng.intended == static_cast(RngCtrl::CONDITIONAL)) + && (_params.ekf2_rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) - && (_fc.rng.intended == static_cast(RngCtrl::ENABLED)); + && (_params.ekf2_rng_ctrl == static_cast(RngCtrl::ENABLED)); if (_control_status.flags.rng_hgt) { if (!(do_conditional_range_aid || do_range_aid)) { diff --git a/src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp b/src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp index ffb1c003be..4412988968 100644 --- a/src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ranging_beacon/ranging_beacon_control.cpp @@ -42,7 +42,9 @@ void Ekf::controlRangingBeaconFusion(const imuSample &imu_delayed) { - if (!_ranging_beacon_buffer || (_params.ekf2_rngbc_ctrl == 0)) { + _fc.rngbcn.available = (_params.ekf2_rngbc_ctrl != 0); + + if (!_ranging_beacon_buffer || !_fc.rngbcn.intended()) { stopRangingBeaconFusion(); return; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 47faab8bfa..19c21baae8 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -287,10 +287,10 @@ struct systemFlagUpdate { // Runtime fusion control. Populated by EKF2 module, read by EKF core. static constexpr uint8_t MAX_AGP_INSTANCES = 4; - struct FusionSensor { - bool exists{false}; - int32_t intended{0}; + bool enabled{false}; // runtime toggleable via MAVLink + bool available{false}; // CTRL-param != disabled-value (functions as factory-setting) + bool intended() const { return enabled && available; } }; struct FusionControl { @@ -300,9 +300,9 @@ struct FusionControl { FusionSensor agp[MAX_AGP_INSTANCES]; FusionSensor baro; FusionSensor rng; - FusionSensor drag; - FusionSensor mag{false, 5}; // MagFuseType::NONE - FusionSensor imu; + FusionSensor mag; + FusionSensor aspd; + FusionSensor rngbcn; }; struct parameters { @@ -314,6 +314,7 @@ struct parameters { float ekf2_vel_lim{100.f}; ///< velocity state limit (m/s) // measurement source control + int32_t ekf2_sens_en{8191}; ///< sensor fusion enable bitmask (EKF2_SENS_EN) int32_t ekf2_hgt_ref{static_cast(HeightSensor::BARO)}; int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; @@ -340,7 +341,7 @@ struct parameters { #if defined(CONFIG_EKF2_BAROMETER) int32_t ekf2_baro_ctrl {1}; - float ekf2_baro_delay{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) + float ekf2_baro_delay {0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) float ekf2_baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) float ekf2_baro_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 0ca9824190..574e7f70c2 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -160,7 +160,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) _zero_velocity_update.update(*this, imu_delayed); - if (_fc.imu.intended & static_cast(ImuCtrl::GyroBias)) { + if (_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GyroBias)) { _zero_gyro_update.update(*this, imu_delayed); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index d1c0dfcf20..cd571e85fe 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -137,6 +137,10 @@ public: const Vector3f &getFlowRefBodyRate() const { return _ref_body_rate; } #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + uint8_t getAgpFusingBitmask() const { return _aux_global_position.sourceFusingBitmask(); } +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + float getHeadingInnov() const; float getHeadingInnovVar() const; float getHeadingInnovRatio() const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 763e474b69..1ea83ceac8 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -826,7 +826,7 @@ void Ekf::updateHorizontalDeadReckoningstatus() inertial_dead_reckoning = false; } else { - if (!_control_status.flags.in_air && (_fc.of.intended == 1) + if (!_control_status.flags.in_air && _fc.of.intended() && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max) ) { // currently landed, but optical flow aiding should be possible once in air @@ -853,7 +853,7 @@ void Ekf::updateHorizontalDeadReckoningstatus() if (!_control_status.flags.in_air && _control_status.flags.fixed_wing && (_params.ekf2_fuse_beta == 1) - && (_params.ekf2_arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max) + && _fc.aspd.intended() && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max) ) { // currently landed, but air data aiding should be possible once in air aiding_expected_in_air = true; @@ -997,7 +997,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) // gyro bias inhibit - const bool do_inhibit_all_gyro_axes = !(_fc.imu.intended & static_cast(ImuCtrl::GyroBias)); + const bool do_inhibit_all_gyro_axes = !(_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GyroBias)); for (unsigned index = 0; index < State::gyro_bias.dof; index++) { bool is_bias_observable = true; // TODO: gyro bias conditions @@ -1005,7 +1005,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) } // accel bias inhibit - const bool do_inhibit_all_accel_axes = !(_fc.imu.intended & static_cast(ImuCtrl::AccelBias)) + const bool do_inhibit_all_accel_axes = !(_params.ekf2_imu_ctrl & static_cast(ImuCtrl::AccelBias)) || is_manoeuvre_level_high || _fault_status.flags.bad_acc_vertical; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 12f35c8660..a6d6ca8038 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -519,7 +519,7 @@ void EstimatorInterface::setDragData(const imuSample &imu) { // down-sample the drag specific force data by accumulating and calculating the mean when // sufficient samples have been collected - if (_fc.drag.intended > 0) { + if (_params.ekf2_drag_ctrl > 0) { // Allocate the required buffer size if not previously done if (_drag_buffer == nullptr) {