fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 03:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: enable creading multi-ekf with different configs

Browse Source
This commit is contained in:
bresch
2023-05-19 18:51:34 +02:00
parent 9803b09a16
commit 7467c5fad5
4 changed files with 104 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+45 -26
View File
@@ -46,11 +46,12 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
#endif // CONFIG_EKF2_MULTI_INSTANCE
EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode, int config_param):
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, config),
_replay_mode(replay_mode && !multi_mode),
_multi_mode(multi_mode),
_config_param(config_param),
_instance(multi_mode ? -1 : 0),
_attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
_local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
@@ -112,7 +113,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_req_vdrift(_params->req_vdrift),
_param_ekf2_hgt_ref(_params->height_sensor_ref),
_param_ekf2_baro_ctrl(_params->baro_ctrl),
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
_param_ekf2_noaid_tout(_params->valid_timeout_max),
#if defined(CONFIG_EKF2_RANGE_FINDER)
_param_ekf2_rng_ctrl(_params->rng_ctrl),
@@ -136,7 +136,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_param_ekf2_ev_delay(_params->ev_delay_ms),
_param_ekf2_ev_ctrl(_params->ev_ctrl),
_param_ekf2_ev_qmin(_params->ev_quality_minimum),
_param_ekf2_evp_noise(_params->ev_pos_noise),
_param_ekf2_evv_noise(_params->ev_vel_noise),
@@ -440,6 +439,22 @@ void EKF2::Run()
}
}
}
if (_config_param == 1) {
_params->gnss_ctrl = _param_ekf2_1_gps_ctrl.get();
} else {
_params->gnss_ctrl = _param_ekf2_gps_ctrl.get();
}
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_config_param == 1) {
_params->ev_ctrl = _param_ekf2_1_ev_ctrl.get();
} else {
_params->ev_ctrl = _param_ekf2_ev_ctrl.get();
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
}
if (!_callback_registered) {
@@ -2560,6 +2575,8 @@ int EKF2::task_spawn(int argc, char *argv[])
bool multi_mode = false;
int32_t imu_instances = 0;
int32_t mag_instances = 0;
int32_t conf_instances = 0;
param_get(param_find("EKF2_MULTI_CONF"), &conf_instances);
int32_t sens_imu_mode = 1;
param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode);
@@ -2624,13 +2641,13 @@ int EKF2::task_spawn(int argc, char *argv[])
}
const hrt_abstime time_started = hrt_absolute_time();
const int multi_instances = math::min(imu_instances * mag_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
const int multi_instances = math::min(imu_instances * mag_instances * conf_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
int multi_instances_allocated = 0;
// allocate EKF2 instances until all found or arming
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS] {}; // IMUs * mags
bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS][MAX_NUM_CONFS] {}; // IMUs * mags * configs
while ((multi_instances_allocated < multi_instances)
&& (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
@@ -2650,34 +2667,36 @@ int EKF2::task_spawn(int argc, char *argv[])
// Mag & IMU data must be valid, first mag can be ignored initially
if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
if (!ekf2_instance_created[imu][mag]) {
EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
for (uint8_t conf = 0; conf < conf_instances; conf++) {
if (!ekf2_instance_created[imu][mag][conf]) {
EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false, conf);
if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
_objects[actual_instance].store(ekf2_inst);
success = true;
multi_instances_allocated++;
ekf2_instance_created[imu][mag] = true;
if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
_objects[actual_instance].store(ekf2_inst);
success = true;
multi_instances_allocated++;
ekf2_instance_created[imu][mag][conf] = true;
PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
imu, vehicle_imu_sub.get().accel_device_id,
mag, vehicle_mag_sub.get().device_id);
PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
imu, vehicle_imu_sub.get().accel_device_id,
mag, vehicle_mag_sub.get().device_id);
_ekf2_selector.load()->ScheduleNow();
_ekf2_selector.load()->ScheduleNow();
} else {
PX4_ERR("instance numbering problem instance: %d", actual_instance);
delete ekf2_inst;
break;
}
} else {
PX4_ERR("instance numbering problem instance: %d", actual_instance);
delete ekf2_inst;
PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
px4_usleep(100000);
break;
}
} else {
PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
px4_usleep(100000);
break;
}
}
@@ -2699,7 +2718,7 @@ int EKF2::task_spawn(int argc, char *argv[])
{
// otherwise launch regular
EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode, 0);
if (ekf2_inst) {
_objects[0].store(ekf2_inst);
src/modules/ekf2/EKF2.hpp
+7 -3
View File
@@ -115,7 +115,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
{
public:
EKF2() = delete;
EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode);
EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode, int config_param);
~EKF2() override;
/** @see ModuleBase */
@@ -160,6 +160,7 @@ private:
static constexpr uint8_t MAX_NUM_IMUS = 4;
static constexpr uint8_t MAX_NUM_MAGS = 4;
static constexpr uint8_t MAX_NUM_CONFS = 2;
void Run() override;
@@ -257,6 +258,7 @@ private:
const bool _replay_mode{false}; ///< true when we use replay data from a log
const bool _multi_mode;
const int _config_param{0};
int _instance{0};
px4::atomic_bool _task_should_exit{false};
@@ -578,7 +580,8 @@ private:
_param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl, ///< GPS control selection
(ParamInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl, ///< GPS control selection
(ParamInt<px4::params::EKF2_1_GPS_CTRL>) _param_ekf2_1_gps_ctrl, ///< Alternate GPS control selection
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
_param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
@@ -626,7 +629,8 @@ private:
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
(ParamExtInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection
(ParamInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection
(ParamInt<px4::params::EKF2_1_EV_CTRL>) _param_ekf2_1_ev_ctrl, ///< Alternate external vision (EV) control selection
(ParamInt<px4::params::EKF2_EV_NOISE_MD>) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)
src/modules/ekf2/ekf2_params.c
+38
View File
@@ -686,6 +686,25 @@ PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1);
*/
PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15);
/**
* (Alt)External vision (EV) sensor aiding
*
* Set bits in the following positions to enable:
* 0 : Horizontal position fusion
* 1 : Vertical position fusion
* 2 : 3D velocity fusion
* 3 : Yaw
*
* @group EKF2
* @min 0
* @max 15
* @bit 0 Horizontal position
* @bit 1 Vertical position
* @bit 2 3D velocity
* @bit 3 Yaw
*/
PARAM_DEFINE_INT32(EKF2_1_EV_CTRL, 15);
/**
* GNSS sensor aiding
*
@@ -705,6 +724,25 @@ PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15);
*/
PARAM_DEFINE_INT32(EKF2_GPS_CTRL, 7);
/**
* (Alt)GNSS sensor aiding
*
* Set bits in the following positions to enable:
* 0 : Longitude and latitude fusion
* 1 : Altitude fusion
* 2 : 3D velocity fusion
* 3 : Dual antenna heading fusion
*
* @group EKF2
* @min 0
* @max 15
* @bit 0 Lon/lat
* @bit 1 Altitude
* @bit 2 3D velocity
* @bit 3 Dual antenna heading
*/
PARAM_DEFINE_INT32(EKF2_1_GPS_CTRL, 7);
/**
* Range sensor height aiding
*
src/modules/ekf2/ekf2_params_multi.c
+14
View File
@@ -56,3 +56,17 @@ PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0);
* @max 4
*/
PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0);
/**
* Multi-EKF Configurations.
*
* Select the number of configurations to use for Multi-EKF.
* Multiple configurations can be used with the same IMU/mag
* Requires SENS_IMU_MODE > 0
*
* @group EKF2
* @reboot_required true
* @min 1
* @max 2
*/
PARAM_DEFINE_INT32(EKF2_MULTI_CONF, 1);