From 52401ea5b81fca37bdf4f59dd5be78c900aac90c Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 19 May 2023 18:51:34 +0200 Subject: [PATCH] ekf2: enable creading multi-ekf with different configs --- src/modules/ekf2/EKF2.cpp | 71 ++++++++++++++++++---------- src/modules/ekf2/EKF2.hpp | 10 ++-- src/modules/ekf2/ekf2_params.c | 38 +++++++++++++++ src/modules/ekf2/ekf2_params_multi.c | 14 ++++++ 4 files changed, 104 insertions(+), 29 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a9fad97550..7f61901d2c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -46,11 +46,12 @@ static px4::atomic _objects[EKF2_MAX_INSTANCES] {}; static px4::atomic _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(EKF2_MAX_INSTANCES)); + const int multi_instances = math::min(imu_instances * mag_instances * conf_instances, static_cast(EKF2_MAX_INSTANCES)); int multi_instances_allocated = 0; // allocate EKF2 instances until all found or arming uORB::SubscriptionData 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); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index a40971e79f..95e47aca25 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -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) _param_ekf2_hgt_ref, ///< selects the primary source for height data (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection - (ParamExtInt) _param_ekf2_gps_ctrl, ///< GPS control selection + (ParamInt) _param_ekf2_gps_ctrl, ///< GPS control selection + (ParamInt) _param_ekf2_1_gps_ctrl, ///< Alternate GPS control selection (ParamExtInt) _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) _param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec) - (ParamExtInt) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection + (ParamInt) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection + (ParamInt) _param_ekf2_1_ev_ctrl, ///< Alternate external vision (EV) control selection (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise (ParamExtInt) _param_ekf2_ev_qmin, (ParamExtFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index cf4a49ff75..2571d76ed7 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -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 * diff --git a/src/modules/ekf2/ekf2_params_multi.c b/src/modules/ekf2/ekf2_params_multi.c index cd06198dc1..5d94127945 100644 --- a/src/modules/ekf2/ekf2_params_multi.c +++ b/src/modules/ekf2/ekf2_params_multi.c @@ -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);