diff --git a/msg/EstimatorFusionControl.msg b/msg/EstimatorFusionControl.msg index 3a90e86c19..e9910f6d53 100644 --- a/msg/EstimatorFusionControl.msg +++ b/msg/EstimatorFusionControl.msg @@ -1,13 +1,23 @@ 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) +# sensor intended for fusion (enabled via EKF2_SENS_EN AND CTRL param != disabled) +bool[2] gps_intended +bool of_intended +bool ev_intended +bool[4] agp_intended +bool baro_intended +bool rng_intended +bool mag_intended +bool aspd_intended +bool rngbcn_intended -uint8 agp_active # bitmask of AGP instances actively fusing (bit i = instance i) +# whether the estimator is actively fusing data from each source +bool[2] gps_active +bool of_active +bool ev_active +bool[4] agp_active +bool baro_active +bool rng_active +bool mag_active +bool aspd_active +bool rngbcn_active diff --git a/msg/versioned/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg index 8564fe7c73..be86cd22df 100644 --- a/msg/versioned/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -110,17 +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) +uint16 VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE = 43006 # Enable/disable estimator sensor fusion. |Source (FUSION_SOURCE_*)|Sensor instance (0-based)|Enable (1) or Disable (0)|Estimator Instance (NaN: not used)|Empty|Empty|Empty| +# Sensor fusion source types for VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE +uint8 FUSION_SOURCE_GPS = 0 # GNSS (EKF2_GPS{i}_CTRL, use instance param) +uint8 FUSION_SOURCE_OF = 1 # Optical Flow (EKF2_OF_CTRL) +uint8 FUSION_SOURCE_EV = 2 # External Vision (EKF2_EV_CTRL) +uint8 FUSION_SOURCE_AGP = 3 # Auxiliary Global Position (EKF2_AGP{i}_CTRL, use instance param) +uint8 FUSION_SOURCE_BARO = 4 # Barometer (EKF2_BARO_CTRL) +uint8 FUSION_SOURCE_RNG = 5 # Range Finder (EKF2_RNG_CTRL) +uint8 FUSION_SOURCE_MAG = 6 # Magnetometer (EKF2_MAG_TYPE) +uint8 FUSION_SOURCE_ASPD = 7 # Airspeed (EKF2_ARSP_THR) +uint8 FUSION_SOURCE_RNGBCN = 8 # Ranging Beacon # 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 cced4cd4bb..36343b97e0 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1613,7 +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: + case vehicle_command_s::VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE: /* ignore commands that are handled by other parts of the system */ break; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index fa2781254e..3061eb2a4f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -67,6 +67,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_delay_max(_params->ekf2_delay_max), _param_ekf2_imu_ctrl(_params->ekf2_imu_ctrl), _param_ekf2_vel_lim(_params->ekf2_vel_lim), + _param_ekf2_sens_en(_params->ekf2_sens_en), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->ekf2_avel_delay), #endif // CONFIG_EKF2_AUXVEL @@ -219,16 +220,6 @@ 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(); } @@ -470,7 +461,7 @@ void EKF2::Run() // update parameters from storage updateParams(); - updateFusionIntended(); + initFusionControl(); VerifyParams(); @@ -621,7 +612,7 @@ void EKF2::Run() _vehicle_command_ack_pub.publish(command_ack); } - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_EKF_SENSOR_FUSION) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE) { handleSensorFusionCommand(vehicle_command, command_ack); command_ack.timestamp = hrt_absolute_time(); _vehicle_command_ack_pub.publish(command_ack); @@ -1006,194 +997,100 @@ 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); - } + if (!_prev_armed) { - // 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; -} + const int32_t sens_en = _param_ekf2_sens_en.get(); -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; + _fc.gps.enabled = sens_en & (1 << SENS_EN_GPS0); + _fc.of.enabled = sens_en & (1 << SENS_EN_OF); + _fc.ev.enabled = sens_en & (1 << SENS_EN_EV); + + for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) { + _fc.agp[i].enabled = sens_en & (1 << (SENS_EN_AGP0 + i)); } - } -#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; + + _fc.baro.enabled = sens_en & (1 << SENS_EN_BARO); + _fc.rng.enabled = sens_en & (1 << SENS_EN_RNG); + _fc.mag.enabled = sens_en & (1 << SENS_EN_MAG); + _fc.aspd.enabled = sens_en & (1 << SENS_EN_ASPD); + _fc.rngbcn.enabled = sens_en & (1 << SENS_EN_RNGBCN); } } - void EKF2::handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack) { - const uint8_t sensor = static_cast(cmd.param1); + ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + + const uint8_t sensor_type = 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 = (static_cast(cmd.param3) == 1); - const bool enable = (action == 1); - bool supported = false; + FusionSensor *sensor = nullptr; - 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; + switch (sensor_type) { + case vehicle_command_s::FUSION_SOURCE_GPS: sensor = &_fc.gps; 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_OF: sensor = &_fc.of; 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_EV: sensor = &_fc.ev; 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; - } + if (instance < MAX_AGP_INSTANCES) { sensor = &_fc.agp[instance]; } + 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_BARO: sensor = &_fc.baro; 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_RNG: sensor = &_fc.rng; 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: sensor = &_fc.mag; 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_ASPD: sensor = &_fc.aspd; 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; + case vehicle_command_s::FUSION_SOURCE_RNGBCN: sensor = &_fc.rngbcn; break; + + default: break; } - ack.result = supported ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + if (sensor) { + sensor->enabled = enable; + + if (!_prev_armed) { + syncSensEnParam(); + } + + ack.result = (!enable || sensor->available) + ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED + : vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } -void EKF2::updateBootParam(uint8_t bit, bool enable) +void EKF2::syncSensEnParam() { - if (_boot_param == PARAM_INVALID) { - return; + int32_t sens_en = 0; + + if (_fc.gps.enabled) { sens_en |= (1 << SENS_EN_GPS0); } + + if (_fc.of.enabled) { sens_en |= (1 << SENS_EN_OF); } + + if (_fc.ev.enabled) { sens_en |= (1 << SENS_EN_EV); } + + for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) { + if (_fc.agp[i].enabled) { sens_en |= (1 << (SENS_EN_AGP0 + i)); } } - int32_t boot_mask = 0; - param_get(_boot_param, &boot_mask); + if (_fc.baro.enabled) { sens_en |= (1 << SENS_EN_BARO); } - if (enable) { - boot_mask |= (1 << bit); - } else { - boot_mask &= ~(1 << bit); - } + if (_fc.rng.enabled) { sens_en |= (1 << SENS_EN_RNG); } - param_set_no_notification(_boot_param, &boot_mask); + if (_fc.mag.enabled) { sens_en |= (1 << SENS_EN_MAG); } + + if (_fc.aspd.enabled) { sens_en |= (1 << SENS_EN_ASPD); } + + if (_fc.rngbcn.enabled) { sens_en |= (1 << SENS_EN_RNGBCN); } + + _param_ekf2_sens_en.set(sens_en); + _param_ekf2_sens_en.commit_no_notification(); } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) @@ -2118,19 +2015,40 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) 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; + msg.gps_intended[0] = _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.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.baro_intended = _fc.baro.intended(); + msg.rng_intended = _fc.rng.intended(); + msg.mag_intended = _fc.mag.intended(); + msg.aspd_intended = _fc.aspd.intended(); + msg.rngbcn_intended = _fc.rngbcn.intended(); + + const auto &cs = _ekf.control_status_flags(); + msg.gps_active[0] = cs.gnss_pos || cs.gps_hgt || cs.gnss_vel || cs.gnss_yaw; + msg.of_active = cs.opt_flow; + msg.ev_active = cs.ev_pos || cs.ev_hgt || cs.ev_vel || cs.ev_yaw; + msg.baro_active = cs.baro_hgt; + msg.rng_active = cs.rng_hgt; + msg.mag_active = cs.mag; + msg.aspd_active = cs.fuse_aspd; + // msg.rngbcn_active = cs.rngbcn_fusion; // waiting for RangeBeacon PR + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) + { + const uint8_t agp_mask = _ekf.getAgpFusingBitmask(); + + for (uint8_t i = 0; i < MAX_AGP_INSTANCES; i++) { + msg.agp_active[i] = agp_mask & (1u << i); + } + } +#endif + msg.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_fc_pub.publish(msg); } @@ -2883,13 +2801,18 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) // vehicle_status vehicle_status_s vehicle_status; - bool armed = false; - if (_status_sub.copy(&vehicle_status) && (ekf2_timestamps.timestamp < vehicle_status.timestamp + 3_s)) { + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (_prev_armed && !armed) { + syncSensEnParam(); + } + + _prev_armed = armed; + // initially set in_air from arming_state (will be overridden if land detector is available) - armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); flags.in_air = armed; // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index e5420ad0eb..b350d9c102 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -407,16 +407,24 @@ 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}; + enum SensEnBit : uint16_t { + SENS_EN_GPS0 = 0, + SENS_EN_GPS1 = 1, + SENS_EN_OF = 2, + SENS_EN_EV = 3, + SENS_EN_AGP0 = 4, + // bit: 5-7 reserved for AGP1..3 + SENS_EN_BARO = 8, + SENS_EN_RNG = 9, + SENS_EN_MAG = 10, + SENS_EN_ASPD = 11, + SENS_EN_RNGBCN = 12, + }; + bool _prev_armed{false}; void initFusionControl(); - void updateFusionIntended(); void handleSensorFusionCommand(const vehicle_command_s &cmd, vehicle_command_ack_s &ack); - void updateBootParam(uint8_t bit, bool enable); + void syncSensEnParam(); uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; @@ -519,6 +527,7 @@ private: (ParamExtInt) _param_ekf2_imu_ctrl, (ParamExtFloat) _param_ekf2_vel_lim, (ParamBool) _param_ekf2_pos_lock, + (ParamExtInt) _param_ekf2_sens_en, #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) @@ -568,7 +577,7 @@ private: #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) - (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection + (ParamExtInt) _param_ekf2_baro_ctrl, (ParamExtFloat) _param_ekf2_baro_delay, (ParamExtFloat) _param_ekf2_baro_noise, (ParamExtFloat) _param_ekf2_baro_gate, @@ -690,7 +699,7 @@ private: #if defined(CONFIG_EKF2_OPTICAL_FLOW) // optical flow fusion (ParamExtInt) - _param_ekf2_of_ctrl, ///< optical flow fusion selection + _param_ekf2_of_ctrl, (ParamExtInt) _param_ekf2_of_gyr_src, (ParamExtFloat) diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index ffec5c27b3..9d04565d3c 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -193,27 +193,29 @@ parameters: type: boolean default: 0 - EKF2_EN_BOOT: + EKF2_SENS_EN: 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.' + short: Sensor fusion enable bitmask + long: 'Bitmask to control which sensor fusion sources are enabled. + Sources whose bit is cleared will be disabled. + Only applied while disarmed. For in-flight changes use the + MAVLink command VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE or the + individual CTRL params (e.g. EKF2_GPS_CTRL, EKF2_BARO_CTRL).' 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 + 0: GNSS 0 + 1: GNSS 1 + 2: Optical flow + 3: External vision + 4: Aux global position 0 + 5: Aux global position 1 + 6: Aux global position 2 + 7: Aux global position 3 + 8: Barometer + 9: Range finder 10: Magnetometer - 11: IMU - default: 4095 + 11: Airspeed + 12: Ranging beacon + default: 8191 min: 0 - max: 4095 + max: 8191