mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
feat(ekf2): EKF2_SENS_EN param, SensEnBit enum, MAVLink fusion command
Add EKF2_SENS_EN bitmask parameter (replaces EKF2_EN_BOOT) with per-sensor enable bits. initFusionControl reads SENS_EN while disarmed. handleSensorFusionCommand sets FusionSensor.enabled via VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE. syncSensEnParam writes back to param on disarm. Update EstimatorFusionControl.msg to bool intended/active fields. Update VehicleCommand.msg FUSION_SOURCE enum.
This commit is contained in:
Marco Hauswirth
Marco Hauswirth
parent
b9a1c429b3
commit
6306c78f79
@ -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
|
||||
|
||||
@ -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).
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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) {
|
||||
|
||||
const int32_t sens_en = _param_ekf2_sens_en.get();
|
||||
|
||||
_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));
|
||||
}
|
||||
|
||||
// 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;
|
||||
_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<uint8_t>(cmd.param1);
|
||||
ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
|
||||
|
||||
const uint8_t sensor_type = static_cast<uint8_t>(cmd.param1);
|
||||
const uint8_t instance = PX4_ISFINITE(cmd.param2) ? static_cast<uint8_t>(cmd.param2) : 0;
|
||||
const int32_t action = static_cast<int32_t>(cmd.param3); // 0 = disable, 1 = enable
|
||||
const bool enable = (static_cast<int32_t>(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)
|
||||
|
||||
@ -407,16 +407,24 @@ private:
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Publication<vehicle_command_ack_s> _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<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
|
||||
(ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim,
|
||||
(ParamBool<px4::params::EKF2_POS_LOCK>) _param_ekf2_pos_lock,
|
||||
(ParamExtInt<px4::params::EKF2_SENS_EN>) _param_ekf2_sens_en,
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
|
||||
@ -568,7 +577,7 @@ private:
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
|
||||
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>) _param_ekf2_baro_delay,
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>) _param_ekf2_baro_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_GATE>) _param_ekf2_baro_gate,
|
||||
@ -690,7 +699,7 @@ private:
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// optical flow fusion
|
||||
(ParamExtInt<px4::params::EKF2_OF_CTRL>)
|
||||
_param_ekf2_of_ctrl, ///< optical flow fusion selection
|
||||
_param_ekf2_of_ctrl,
|
||||
(ParamExtInt<px4::params::EKF2_OF_GYR_SRC>)
|
||||
_param_ekf2_of_gyr_src,
|
||||
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user