diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 967a6627b2f..17d55ab740b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -213,18 +213,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_abl_tau(_params->acc_bias_learn_tc), _param_ekf2_gyr_b_lim(_params->gyro_bias_lim) { - // advertise expected minimal topic set immediately to ensure logging - _attitude_pub.advertise(); - _local_position_pub.advertise(); - - _estimator_event_flags_pub.advertise(); - _estimator_innovation_test_ratios_pub.advertise(); - _estimator_innovation_variances_pub.advertise(); - _estimator_innovations_pub.advertise(); - _estimator_sensor_bias_pub.advertise(); - _estimator_states_pub.advertise(); - _estimator_status_flags_pub.advertise(); - _estimator_status_pub.advertise(); + AdvertiseTopics(); } EKF2::~EKF2() @@ -233,120 +222,164 @@ EKF2::~EKF2() perf_free(_msg_missed_imu_perf); } -#if defined(CONFIG_EKF2_MULTI_INSTANCE) -bool EKF2::multi_init(int imu, int mag) +void EKF2::AdvertiseTopics() { - // advertise all topics to ensure consistent uORB instance numbering + // advertise expected minimal topic set immediately for logging + _attitude_pub.advertise(); + _local_position_pub.advertise(); _estimator_event_flags_pub.advertise(); - _estimator_innovation_test_ratios_pub.advertise(); - _estimator_innovation_variances_pub.advertise(); - _estimator_innovations_pub.advertise(); -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _estimator_optical_flow_vel_pub.advertise(); -#endif // CONFIG_EKF2_OPTICAL_FLOW _estimator_sensor_bias_pub.advertise(); - _estimator_states_pub.advertise(); - _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); + _estimator_status_flags_pub.advertise(); + + if (_multi_mode) { + // only force advertise these in multi mode to ensure consistent uORB instance numbering + _global_position_pub.advertise(); + _odometry_pub.advertise(); + +#if defined(CONFIG_EKF2_WIND) + _wind_pub.advertise(); +#endif // CONFIG_EKF2_WIND + } #if defined(CONFIG_EKF2_GNSS) - _yaw_est_pub.advertise(); + + if (_param_ekf2_gps_ctrl.get()) { + _estimator_gps_status_pub.advertise(); + _yaw_est_pub.advertise(); + } + #endif // CONFIG_EKF2_GNSS + // verbose logging + if (_param_ekf2_log_verbose.get()) { + _estimator_innovation_test_ratios_pub.advertise(); + _estimator_innovation_variances_pub.advertise(); + _estimator_innovations_pub.advertise(); + _estimator_states_pub.advertise(); + +#if defined(CONFIG_EKF2_AIRSPEED) + + if (_param_ekf2_arsp_thr.get() > 0.f) { + _estimator_aid_src_airspeed_pub.advertise(); + } + +#endif // CONFIG_EKF2_AIRSPEED + #if defined(CONFIG_EKF2_BAROMETER) - // baro advertise - if (_param_ekf2_baro_ctrl.get()) { - _estimator_aid_src_baro_hgt_pub.advertise(); - _estimator_baro_bias_pub.advertise(); - } + if (_param_ekf2_baro_ctrl.get()) { + _estimator_aid_src_baro_hgt_pub.advertise(); + _estimator_baro_bias_pub.advertise(); + } #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_DRAG_FUSION) + + if (_param_ekf2_drag_ctrl.get()) { + _estimator_aid_src_drag_pub.advertise(); + } + +#endif // CONFIG_EKF2_DRAG_FUSION + #if defined(CONFIG_EKF2_EXTERNAL_VISION) - // EV advertise - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS)) { - _estimator_aid_src_ev_hgt_pub.advertise(); - _estimator_ev_pos_bias_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS)) { + _estimator_aid_src_ev_hgt_pub.advertise(); + _estimator_ev_pos_bias_pub.advertise(); + } - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::HPOS)) { - _estimator_aid_src_ev_pos_pub.advertise(); - _estimator_ev_pos_bias_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::HPOS)) { + _estimator_aid_src_ev_pos_pub.advertise(); + _estimator_ev_pos_bias_pub.advertise(); + } - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VEL)) { - _estimator_aid_src_ev_vel_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VEL)) { + _estimator_aid_src_ev_vel_pub.advertise(); + } - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::YAW)) { - _estimator_aid_src_ev_yaw_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::YAW)) { + _estimator_aid_src_ev_yaw_pub.advertise(); + } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - // GNSS advertise - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { - _estimator_aid_src_gnss_hgt_pub.advertise(); - _estimator_gnss_hgt_bias_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get()) { + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { + _estimator_aid_src_gnss_hgt_pub.advertise(); + _estimator_gnss_hgt_bias_pub.advertise(); + } - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::HPOS)) { - _estimator_aid_src_gnss_pos_pub.advertise(); - _estimator_gps_status_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::HPOS)) { + _estimator_aid_src_gnss_pos_pub.advertise(); + } - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VEL)) { - _estimator_aid_src_gnss_vel_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VEL)) { + _estimator_aid_src_gnss_vel_pub.advertise(); + } # if defined(CONFIG_EKF2_GNSS_YAW) - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::YAW)) { - _estimator_aid_src_gnss_yaw_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::YAW)) { + _estimator_aid_src_gnss_yaw_pub.advertise(); + } # endif // CONFIG_EKF2_GNSS_YAW + } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_GRAVITY_FUSION) - if (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::GravityVector)) { - _estimator_aid_src_gravity_pub.advertise(); - } + if (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::GravityVector)) { + _estimator_aid_src_gravity_pub.advertise(); + } #endif // CONFIG_EKF2_GRAVITY_FUSION -#if defined(CONFIG_EKF2_RANGE_FINDER) - - // RNG advertise - if (_param_ekf2_rng_ctrl.get()) { - _estimator_aid_src_rng_hgt_pub.advertise(); - } - -#endif // CONFIG_EKF2_RANGE_FINDER - #if defined(CONFIG_EKF2_MAGNETOMETER) - // mag advertise - if (_param_ekf2_mag_type.get() != MagFuseType::NONE) { - _estimator_aid_src_mag_pub.advertise(); - } + if (_param_ekf2_mag_type.get() != MagFuseType::NONE) { + _estimator_aid_src_mag_pub.advertise(); + } #endif // CONFIG_EKF2_MAGNETOMETER - _attitude_pub.advertise(); - _local_position_pub.advertise(); - _global_position_pub.advertise(); - _odometry_pub.advertise(); +#if defined(CONFIG_EKF2_OPTICAL_FLOW) -#if defined(CONFIG_EKF2_WIND) - _wind_pub.advertise(); -#endif // CONFIG_EKF2_WIND + if (_param_ekf2_of_ctrl.get()) { + _estimator_optical_flow_vel_pub.advertise(); + _estimator_aid_src_optical_flow_pub.advertise(); + } +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + // RNG advertise + if (_param_ekf2_rng_ctrl.get()) { + _estimator_aid_src_rng_hgt_pub.advertise(); + } + +#endif // CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_SIDESLIP) + + if (_param_ekf2_fuse_beta.get()) { + _estimator_aid_src_sideslip_pub.advertise(); + } + +#endif // CONFIG_EKF2_SIDESLIP + + } // end verbose logging +} + +#if defined(CONFIG_EKF2_MULTI_INSTANCE) +bool EKF2::multi_init(int imu, int mag) +{ bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu); #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -415,6 +448,9 @@ void EKF2::Run() VerifyParams(); + // force advertise topics immediately for logging (EKF2_LOG_VERBOSE, per aid source control) + AdvertiseTopics(); + #if defined(CONFIG_EKF2_GNSS) _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); #endif // CONFIG_EKF2_GNSS @@ -714,24 +750,31 @@ void EKF2::Run() // publish status/logging messages PublishEventFlags(now); - PublishInnovations(now); - PublishInnovationTestRatios(now); - PublishInnovationVariances(now); - PublishStates(now); PublishStatus(now); PublishStatusFlags(now); - PublishAidSourceStatus(now); + + if (_param_ekf2_log_verbose.get()) { + PublishAidSourceStatus(now); + PublishInnovations(now); + PublishInnovationTestRatios(now); + PublishInnovationVariances(now); + PublishStates(now); #if defined(CONFIG_EKF2_BAROMETER) - PublishBaroBias(now); + PublishBaroBias(now); #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) - PublishEvPosBias(now); + PublishEvPosBias(now); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - PublishGnssHgtBias(now); + PublishGnssHgtBias(now); +#endif // CONFIG_EKF2_GNSS + + } + +#if defined(CONFIG_EKF2_GNSS) PublishGpsStatus(now); PublishYawEstimatorStatus(now); #endif // CONFIG_EKF2_GNSS diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index fc50610fd28..2de08049cb5 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -162,6 +162,7 @@ private: void Run() override; + void AdvertiseTopics(); void VerifyParams(); void PublishAidSourceStatus(const hrt_abstime ×tamp); @@ -482,6 +483,7 @@ private: parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) DEFINE_PARAMETERS( + (ParamBool) _param_ekf2_log_verbose, (ParamExtInt) _param_ekf2_predict_us, (ParamExtFloat) _param_ekf2_delay_max, (ParamExtInt) _param_ekf2_imu_ctrl, diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index a2cf1a5e139..be11bd97b35 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -1335,3 +1335,8 @@ parameters: min: 1.0 unit: SD decimal: 1 + EKF2_LOG_VERBOSE: + description: + short: Verbose logging + type: boolean + default: 1 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index fedadaf4745..7dd0a43e940 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -150,65 +150,27 @@ void LoggedTopics::add_default_topics() add_topic_multi("timesync_status", 1000, 3); add_optional_topic_multi("telemetry_status", 1000, 4); - // EKF multi topics (currently max 9 estimators) -#if CONSTRAINED_MEMORY - static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1; -#else - static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed - add_optional_topic("estimator_selector_status"); - add_optional_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); -#endif + // EKF multi topics + { + // optionally log all estimator* topics at minimal rate + const uint16_t kEKFVerboseIntervalMilliseconds = 500; // 2 Hz + const struct orb_metadata *const *topic_list = orb_get_topics(); - // always add the first instance - add_topic("estimator_baro_bias", 500); - add_topic("estimator_gnss_hgt_bias", 500); - add_topic("estimator_ev_pos_bias", 500); - add_topic("estimator_event_flags", 0); - add_topic("estimator_gps_status", 1000); - add_topic("estimator_innovation_test_ratios", 500); - add_topic("estimator_innovation_variances", 500); - add_topic("estimator_innovations", 500); - add_topic("estimator_optical_flow_vel", 200); - add_topic("estimator_sensor_bias", 0); - add_topic("estimator_states", 1000); - add_topic("estimator_status", 200); - add_topic("estimator_status_flags", 0); - add_topic("yaw_estimator_status", 1000); + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strncmp(topic_list[i]->o_name, "estimator", 9) == 0) { + add_optional_topic_multi(topic_list[i]->o_name, kEKFVerboseIntervalMilliseconds); + } + } + } - add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); - - // add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES); + // important EKF topics (higher rate) + add_optional_topic("estimator_selector_status", 10); + add_optional_topic_multi("estimator_event_flags", 10); + add_optional_topic_multi("estimator_optical_flow_vel", 200); + add_optional_topic_multi("estimator_sensor_bias", 1000); + add_optional_topic_multi("estimator_status", 200); + add_optional_topic_multi("estimator_status_flags", 10); + add_optional_topic_multi("yaw_estimator_status", 1000); // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); @@ -262,42 +224,23 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_local_position_groundtruth", 20); // EKF replay - add_topic("estimator_baro_bias"); - add_topic("estimator_gnss_hgt_bias"); - add_topic("estimator_ev_pos_bias"); - add_topic("estimator_event_flags"); - add_topic("estimator_gps_status"); - add_topic("estimator_innovation_test_ratios"); - add_topic("estimator_innovation_variances"); - add_topic("estimator_innovations"); - add_topic("estimator_optical_flow_vel"); - add_topic("estimator_sensor_bias"); - add_topic("estimator_states"); - add_topic("estimator_status"); - add_topic("estimator_status_flags"); + { + // optionally log all estimator* topics at minimal rate + const uint16_t kEKFVerboseIntervalMilliseconds = 10; // 100 Hz + const struct orb_metadata *const *topic_list = orb_get_topics(); + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strncmp(topic_list[i]->o_name, "estimator", 9) == 0) { + add_optional_topic_multi(topic_list[i]->o_name, kEKFVerboseIntervalMilliseconds); + } + } + } + add_topic("vehicle_attitude"); add_topic("vehicle_global_position"); add_topic("vehicle_local_position"); add_topic("wind"); - add_topic("yaw_estimator_status"); - - add_optional_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_vel", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ }