diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fe88726d5f..3ecb1ee597 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -79,6 +79,7 @@ set(msg_files EstimatorBias.msg EstimatorBias3d.msg EstimatorEventFlags.msg + EstimatorFusionControl.msg EstimatorGpsStatus.msg EstimatorInnovations.msg EstimatorSelectorStatus.msg diff --git a/msg/EstimatorFusionControl.msg b/msg/EstimatorFusionControl.msg new file mode 100644 index 0000000000..3a90e86c19 --- /dev/null +++ b/msg/EstimatorFusionControl.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) + +int32 gps_intended # effective EKF2_GPS_CTRL value (0 = disabled) +int32 of_intended # effective EKF2_OF_CTRL value (0 = disabled) +int32 ev_intended # effective EKF2_EV_CTRL value (0 = disabled) +int32[4] agp_intended # effective EKF2_AGPx_CTRL values (0 = disabled) +int32 baro_intended # effective EKF2_BARO_CTRL value (0 = disabled) +int32 rng_intended # effective EKF2_RNG_CTRL value (0 = disabled) +int32 drag_intended # effective EKF2_DRAG_CTRL value (0 = disabled) +int32 mag_intended # effective EKF2_MAG_TYPE value (5 = disabled/NONE) +int32 imu_intended # effective EKF2_IMU_CTRL value (0 = disabled) + +uint8 agp_active # bitmask of AGP instances actively fusing (bit i = instance i) diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 6ea47acc27..8564fe7c73 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -110,6 +110,17 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # External reset of estimator global position when dead reckoning. uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004 +uint16 VEHICLE_CMD_SET_EKF_SENSOR_FUSION = 43005 # Enable/disable EKF sensor fusion. |sensor (FUSION_SOURCE_*)|instance (0-based, for multi-instance types)|action (0=disable, 1=enable)|Unused|Unused|Unused|Unused| +# Sensor fusion source types for VEHICLE_CMD_SET_EKF_SENSOR_FUSION +uint8 FUSION_SOURCE_GPS = 1 # GNSS (EKF2_GPS_CTRL) +uint8 FUSION_SOURCE_OF = 2 # Optical Flow (EKF2_OF_CTRL) +uint8 FUSION_SOURCE_EV = 3 # External Vision (EKF2_EV_CTRL) +uint8 FUSION_SOURCE_AGP = 4 # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param) +uint8 FUSION_SOURCE_BARO = 5 # Barometer (EKF2_BARO_CTRL) +uint8 FUSION_SOURCE_RNG = 6 # Range Finder (EKF2_RNG_CTRL) +uint8 FUSION_SOURCE_DRAG = 7 # Drag (EKF2_DRAG_CTRL) +uint8 FUSION_SOURCE_MAG = 8 # Magnetometer (EKF2_MAG_TYPE) +uint8 FUSION_SOURCE_IMU = 9 # IMU (EKF2_IMU_CTRL) # PX4 vehicle commands (beyond 16 bit MAVLink commands). uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # Start of PX4 internal only vehicle commands (> UINT16_MAX). diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 105ca39168..cced4cd4bb 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1613,6 +1613,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION: case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE: case vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE: + case vehicle_command_s::VEHICLE_CMD_SET_EKF_SENSOR_FUSION: /* ignore commands that are handled by other parts of the system */ break; 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 4c9ba8c086..497eb586a9 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 && (_params.ekf2_drag_ctrl == 0))) { + if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_fc.drag.intended == 0))) { _control_status.flags.wind = false; } 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 3a790cd8f0..b04df2ee83 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 @@ -103,7 +103,7 @@ bool AgpSource::update(Ekf &ekf, const estimator::imuSample &imu_delayed) if (_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) { - if (!(_params.ctrl & static_cast(Ctrl::kHPos))) { + if (!(ekf._fc.agp[_slot].intended & static_cast(Ctrl::kHPos))) { 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 b7b45d5ced..7dce041475 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 @@ -111,7 +111,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) } // determine if we should use height aiding - const bool continuing_conditions_passing = (_params.ekf2_baro_ctrl == 1) + const bool continuing_conditions_passing = (_fc.baro.intended == 1) && 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 de496a9fab..9ed7b7ae67 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 ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) { + if ((_fc.drag.intended > 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_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 215487e430..527ec5014f 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 = (_params.ekf2_ev_ctrl & static_cast(EvCtrl::VPOS)) + const bool continuing_conditions_passing = (_fc.ev.intended & 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 4720824e32..d07d31f894 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 = (_params.ekf2_ev_ctrl & static_cast(EvCtrl::HPOS)) + bool continuing_conditions_passing = (_fc.ev.intended & 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 efc714964b..0989af3956 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 = (_params.ekf2_ev_ctrl & static_cast(EvCtrl::VEL)) + bool continuing_conditions_passing = (_fc.ev.intended & 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 5f597da7ba..0d419ad741 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 = (_params.ekf2_ev_ctrl & static_cast(EvCtrl::YAW)) + bool continuing_conditions_passing = (_fc.ev.intended & 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 ccfd3d3d5a..5c923abe42 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 = (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::VPOS)) + const bool continuing_conditions_passing = (_fc.gps.intended & 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 709fb76211..8233eee1a1 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 (!(_params.ekf2_gps_ctrl & static_cast(GnssCtrl::YAW)) + if (!(_fc.gps.intended & 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 68963be0cc..c8ff42f4ae 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,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) { - if (!_gps_buffer || (_params.ekf2_gps_ctrl == 0)) { + if (!_gps_buffer || (_fc.gps.intended == 0)) { stopGnssFusion(); return; } @@ -126,7 +126,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 = (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::VEL)) + const bool continuing_conditions_passing = (_fc.gps.intended & static_cast(GnssCtrl::VEL)) && _control_status.flags.tilt_align && _control_status.flags.yaw_align && !_control_status.flags.gnss_fault @@ -182,7 +182,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 = (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::HPOS)); + const bool gnss_pos_enabled = (_fc.gps.intended & static_cast(GnssCtrl::HPOS)); const bool continuing_conditions_passing = gnss_pos_enabled && _control_status.flags.tilt_align @@ -385,8 +385,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 (((_params.ekf2_gps_ctrl & static_cast(GnssCtrl::VEL)) - || (_params.ekf2_gps_ctrl & static_cast(GnssCtrl::HPOS))) + if (((_fc.gps.intended & static_cast(GnssCtrl::VEL)) + || (_fc.gps.intended & 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 fb0b3bc932..6c4270ba98 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 = (_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GravityVector)) + _control_status.flags.gravity_vector = (_fc.imu.intended & 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 06dd0b4c80..7069190a82 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,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) _control_status.flags.mag_aligned_in_flight = false; } - if (_params.ekf2_mag_type == MagFuseType::NONE) { + if (_fc.mag.intended == MagFuseType::NONE) { stopMagFusion(); return; } @@ -150,9 +150,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 = ((_params.ekf2_mag_type == MagFuseType::INIT) - || (_params.ekf2_mag_type == MagFuseType::AUTO) - || (_params.ekf2_mag_type == MagFuseType::HEADING)) + bool continuing_conditions_passing = ((_fc.mag.intended == MagFuseType::INIT) + || (_fc.mag.intended == MagFuseType::AUTO) + || (_fc.mag.intended == 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 +187,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 - && (_params.ekf2_mag_type == MagFuseType::AUTO) + && (_fc.mag.intended == MagFuseType::AUTO) && _control_status.flags.mag_aligned_in_flight; _control_status.flags.mag_hdg = common_conditions_passing - && ((_params.ekf2_mag_type == MagFuseType::HEADING) - || (_params.ekf2_mag_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); + && ((_fc.mag.intended == MagFuseType::HEADING) + || (_fc.mag.intended == 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 fa6f70ab4e..fd31089a14 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,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) { - if (!_flow_buffer || (_params.ekf2_of_ctrl != 1)) { + if (!_flow_buffer || (_fc.of.intended != 1)) { stopFlowFusion(); return; } @@ -146,7 +146,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 = (_params.ekf2_of_ctrl == 1) + const bool continuing_conditions_passing = (_fc.of.intended == 1) && _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 3e092e8342..61fe5a95d2 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 @@ -127,8 +127,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) aid_src.innovation_rejected = false; } - const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast(RngCtrl::ENABLED)) - || (_params.ekf2_rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) + const bool continuing_conditions_passing = ((_fc.rng.intended == static_cast(RngCtrl::ENABLED)) + || (_fc.rng.intended == static_cast(RngCtrl::CONDITIONAL))) && _control_status.flags.tilt_align && measurement_valid; @@ -138,11 +138,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) - && (_params.ekf2_rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) + && (_fc.rng.intended == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) - && (_params.ekf2_rng_ctrl == static_cast(RngCtrl::ENABLED)); + && (_fc.rng.intended == 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/common.h b/src/modules/ekf2/EKF/common.h index 0c29ecf6bd..47faab8bfa 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -284,6 +284,27 @@ struct systemFlagUpdate { bool in_transition{false}; }; +// 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}; +}; + +struct FusionControl { + FusionSensor gps; + FusionSensor of; + FusionSensor ev; + FusionSensor agp[MAX_AGP_INSTANCES]; + FusionSensor baro; + FusionSensor rng; + FusionSensor drag; + FusionSensor mag{false, 5}; // MagFuseType::NONE + FusionSensor imu; +}; + struct parameters { int32_t ekf2_predict_us{10000}; ///< filter update interval in microseconds diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 574e7f70c2..0ca9824190 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 (_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GyroBias)) { + if (_fc.imu.intended & static_cast(ImuCtrl::GyroBias)) { _zero_gyro_update.update(*this, imu_delayed); } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index c89d72ba11..763e474b69 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 && (_params.ekf2_of_ctrl == 1) + if (!_control_status.flags.in_air && (_fc.of.intended == 1) && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max) ) { // currently landed, but optical flow aiding should be possible once in air @@ -997,7 +997,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) // gyro bias inhibit - const bool do_inhibit_all_gyro_axes = !(_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GyroBias)); + const bool do_inhibit_all_gyro_axes = !(_fc.imu.intended & 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 = !(_params.ekf2_imu_ctrl & static_cast(ImuCtrl::AccelBias)) + const bool do_inhibit_all_accel_axes = !(_fc.imu.intended & 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 a6d6ca8038..12f35c8660 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 (_params.ekf2_drag_ctrl > 0) { + if (_fc.drag.intended > 0) { // Allocate the required buffer size if not previously done if (_drag_buffer == nullptr) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index f00c295465..93756090f4 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -155,6 +155,7 @@ public: // return a address to the parameters struct // in order to give access to the application parameters *getParamHandle() { return &_params; } + FusionControl *getFusionControlHandle() { return &_fc; } // set vehicle landed status data void set_in_air_status(bool in_air) @@ -336,6 +337,7 @@ protected: virtual bool init(uint64_t timestamp) = 0; parameters _params{}; // filter parameters + FusionControl _fc{}; /* OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b6301898db..fa2781254e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -62,6 +62,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)), #endif // CONFIG_EKF2_WIND _params(_ekf.getParamHandle()), + _fc(*_ekf.getFusionControlHandle()), _param_ekf2_predict_us(_params->ekf2_predict_us), _param_ekf2_delay_max(_params->ekf2_delay_max), _param_ekf2_imu_ctrl(_params->ekf2_imu_ctrl), @@ -218,6 +219,17 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_abl_tau(_params->ekf2_abl_tau), _param_ekf2_gyr_b_lim(_params->ekf2_gyr_b_lim) { +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) + char param_name[20] {}; + + for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) { + snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_CTRL", i); + _param_ekf2_agp_ctrl[i] = param_find(param_name); + } + +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + + initFusionControl(); AdvertiseTopics(); } @@ -458,6 +470,7 @@ void EKF2::Run() // update parameters from storage updateParams(); + updateFusionIntended(); VerifyParams(); @@ -607,6 +620,12 @@ void EKF2::Run() command_ack.timestamp = hrt_absolute_time(); _vehicle_command_ack_pub.publish(command_ack); } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_EKF_SENSOR_FUSION) { + handleSensorFusionCommand(vehicle_command, command_ack); + command_ack.timestamp = hrt_absolute_time(); + _vehicle_command_ack_pub.publish(command_ack); + } } } @@ -828,6 +847,7 @@ void EKF2::Run() PublishEventFlags(now); PublishStatus(now); PublishStatusFlags(now); + PublishFusionControl(now); if (_param_ekf2_log_verbose.get()) { PublishAidSourceStatus(now); @@ -984,6 +1004,198 @@ void EKF2::VerifyParams() } } +void EKF2::initFusionControl() +{ + _boot_param = param_find("EKF2_EN_BOOT"); + int32_t boot_mask = 4095; // default: all enabled + if (_boot_param != PARAM_INVALID) { + param_get(_boot_param, &boot_mask); + } + + // bit layout: 0=GPS 1=OF 2=EV 3..6=AGP0..3 7=BARO 8=RNG 9=DRAG 10=MAG 11=IMU +#if defined(CONFIG_EKF2_GNSS) + _fc.gps.exists = boot_mask & (1 << 0); + _fc.gps.intended = _fc.gps.exists ? _params->ekf2_gps_ctrl : 0; +#endif +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + _fc.of.exists = boot_mask & (1 << 1); + _fc.of.intended = _fc.of.exists ? _params->ekf2_of_ctrl : 0; +#endif +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _fc.ev.exists = boot_mask & (1 << 2); + _fc.ev.intended = _fc.ev.exists ? _params->ekf2_ev_ctrl : 0; +#endif +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) + for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) { + _fc.agp[i].exists = boot_mask & (1 << (3 + i)); + int32_t agp_param_val = 0; + if (_param_ekf2_agp_ctrl[i] != PARAM_INVALID) { + param_get(_param_ekf2_agp_ctrl[i], &agp_param_val); + } + _fc.agp[i].intended = _fc.agp[i].exists ? agp_param_val : 0; + } +#endif +#if defined(CONFIG_EKF2_BAROMETER) + _fc.baro.exists = boot_mask & (1 << 7); + _fc.baro.intended = _fc.baro.exists ? _params->ekf2_baro_ctrl : 0; +#endif +#if defined(CONFIG_EKF2_RANGE_FINDER) + _fc.rng.exists = boot_mask & (1 << 8); + _fc.rng.intended = _fc.rng.exists ? _params->ekf2_rng_ctrl : 0; +#endif +#if defined(CONFIG_EKF2_DRAG_FUSION) + _fc.drag.exists = boot_mask & (1 << 9); + _fc.drag.intended = _fc.drag.exists ? _params->ekf2_drag_ctrl : 0; +#endif +#if defined(CONFIG_EKF2_MAGNETOMETER) + _fc.mag.exists = boot_mask & (1 << 10); + _fc.mag.intended = _fc.mag.exists ? _params->ekf2_mag_type : 5; +#endif + _fc.imu.exists = boot_mask & (1 << 11); + _fc.imu.intended = _fc.imu.exists ? _params->ekf2_imu_ctrl : 0; +} + +void EKF2::updateFusionIntended() +{ + if (_fc.gps.intended != 0) { + _fc.gps.intended = _params->ekf2_gps_ctrl; + } + if (_fc.of.intended != 0) { + _fc.of.intended = _params->ekf2_of_ctrl; + } + if (_fc.ev.intended != 0) { + _fc.ev.intended = _params->ekf2_ev_ctrl; + } +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) + for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) { + if (_fc.agp[i].intended != 0) { + int32_t agp_param_val = 0; + if (_param_ekf2_agp_ctrl[i] != PARAM_INVALID) { + param_get(_param_ekf2_agp_ctrl[i], &agp_param_val); + } + _fc.agp[i].intended = agp_param_val; + } + } +#endif + if (_fc.baro.intended != 0) { + _fc.baro.intended = _params->ekf2_baro_ctrl; + } + if (_fc.rng.intended != 0) { + _fc.rng.intended = _params->ekf2_rng_ctrl; + } + if (_fc.drag.intended != 0) { + _fc.drag.intended = _params->ekf2_drag_ctrl; + } + if (_fc.mag.intended != 5) { + _fc.mag.intended = _params->ekf2_mag_type; + } + if (_fc.imu.intended != 0) { + _fc.imu.intended = _params->ekf2_imu_ctrl; + } +} + + +void EKF2::handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack) +{ + const uint8_t sensor = static_cast(cmd.param1); + const uint8_t instance = PX4_ISFINITE(cmd.param2) ? static_cast(cmd.param2) : 0; + const int32_t action = static_cast(cmd.param3); // 0 = disable, 1 = enable + + const bool enable = (action == 1); + bool supported = false; + + switch (sensor) { + case vehicle_command_s::FUSION_SOURCE_GPS: + _fc.gps.exists = enable; + _fc.gps.intended = enable ? _param_ekf2_gps_ctrl.get() : 0; + updateBootParam(0, enable); + supported = true; + break; + + case vehicle_command_s::FUSION_SOURCE_OF: + _fc.of.exists = enable; + _fc.of.intended = enable ? _param_ekf2_of_ctrl.get() : 0; + updateBootParam(1, enable); + supported = true; + break; + + case vehicle_command_s::FUSION_SOURCE_EV: + _fc.ev.exists = enable; + _fc.ev.intended = enable ? _param_ekf2_ev_ctrl.get() : 0; + updateBootParam(2, enable); + supported = true; + break; + + case vehicle_command_s::FUSION_SOURCE_AGP: + if (instance < MAX_AGP_INSTANCES) { + _fc.agp[instance].exists = enable; + int32_t agp_param_val = 0; + if (_param_ekf2_agp_ctrl[instance] != PARAM_INVALID) { + param_get(_param_ekf2_agp_ctrl[instance], &agp_param_val); + } + _fc.agp[instance].intended = enable ? agp_param_val : 0; + updateBootParam(3 + instance, enable); + supported = true; + } + break; + + case vehicle_command_s::FUSION_SOURCE_BARO: + _fc.baro.exists = enable; + _fc.baro.intended = enable ? _param_ekf2_baro_ctrl.get() : 0; + updateBootParam(7, enable); + supported = true; + break; + + case vehicle_command_s::FUSION_SOURCE_RNG: + _fc.rng.exists = enable; + _fc.rng.intended = enable ? _param_ekf2_rng_ctrl.get() : 0; + updateBootParam(8, enable); + supported = true; + break; + + case vehicle_command_s::FUSION_SOURCE_DRAG: + _fc.drag.exists = enable; + _fc.drag.intended = enable ? _param_ekf2_drag_ctrl.get() : 0; + updateBootParam(9, enable); + supported = true; + break; + + case vehicle_command_s::FUSION_SOURCE_MAG: + _fc.mag.exists = enable; + _fc.mag.intended = enable ? _param_ekf2_mag_type.get() : 5; + updateBootParam(10, enable); + supported = true; + break; + + case vehicle_command_s::FUSION_SOURCE_IMU: + _fc.imu.exists = enable; + _fc.imu.intended = enable ? _param_ekf2_imu_ctrl.get() : 0; + updateBootParam(11, enable); + supported = true; + break; + } + + ack.result = supported ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; +} + +void EKF2::updateBootParam(uint8_t bit, bool enable) +{ + if (_boot_param == PARAM_INVALID) { + return; + } + + int32_t boot_mask = 0; + param_get(_boot_param, &boot_mask); + + if (enable) { + boot_mask |= (1 << bit); + } else { + boot_mask &= ~(1 << bit); + } + + param_set_no_notification(_boot_param, &boot_mask); +} + void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) { #if defined(CONFIG_EKF2_AIRSPEED) @@ -1903,6 +2115,26 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _estimator_status_pub.publish(status); } +void EKF2::PublishFusionControl(const hrt_abstime ×tamp) +{ + estimator_fusion_control_s msg{}; + msg.gps_intended = _fc.gps.intended; + msg.of_intended = _fc.of.intended; + msg.ev_intended = _fc.ev.intended; + + for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) { + msg.agp_intended[i] = _fc.agp[i].intended; + } + + msg.baro_intended = _fc.baro.intended; + msg.rng_intended = _fc.rng.intended; + msg.drag_intended = _fc.drag.intended; + msg.mag_intended = _fc.mag.intended; + msg.imu_intended = _fc.imu.intended; + msg.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_fc_pub.publish(msg); +} + void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) { // publish at ~ 1 Hz (or immediately if filter control status or fault status changes) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 56b3c56746..e5420ad0eb 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -198,6 +199,7 @@ private: void PublishStates(const hrt_abstime ×tamp); void PublishStatus(const hrt_abstime ×tamp); void PublishStatusFlags(const hrt_abstime ×tamp); + void PublishFusionControl(const hrt_abstime ×tamp); #if defined(CONFIG_EKF2_WIND) void PublishWindEstimate(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_WIND @@ -405,6 +407,17 @@ private: uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) + param_t _param_ekf2_agp_ctrl[MAX_AGP_INSTANCES] {PARAM_INVALID, PARAM_INVALID, PARAM_INVALID, PARAM_INVALID}; +#endif + + param_t _boot_param{PARAM_INVALID}; + + void initFusionControl(); + void updateFusionIntended(); + void handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack); + void updateBootParam(uint8_t bit, bool enable); + uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; @@ -443,6 +456,7 @@ private: uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; + uORB::PublicationMulti _estimator_fc_pub{ORB_ID(estimator_fusion_control)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; @@ -496,6 +510,7 @@ private: Ekf _ekf; parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) + FusionControl &_fc; DEFINE_PARAMETERS( (ParamBool) _param_ekf2_log_verbose, diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index 763ceab9c8..ffec5c27b3 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -192,3 +192,28 @@ parameters: long: When enabled, constant position fusion is enabled when the vehicle is landeded if position has been initialized but has currently no vel/pos aiding. type: boolean default: 0 + + EKF2_EN_BOOT: + description: + short: Sensor fusion enable at boot + long: 'Bitmask to control which sensor fusion sources are active at boot. + Sources whose bit is cleared will be disabled at startup. + They can be toggled at runtime via MAVLink command + (VEHICLE_CMD_SET_EKF_SENSOR_FUSION), which also updates this param.' + type: bitmask + bit: + 0: GNSS + 1: Optical flow + 2: External vision + 3: Aux global position 0 + 4: Aux global position 1 + 5: Aux global position 2 + 6: Aux global position 3 + 7: Barometer + 8: Range finder + 9: Drag + 10: Magnetometer + 11: IMU + default: 4095 + min: 0 + max: 4095