Skip to content

Commit

Permalink
ekf2: verbose logging control (new EKF2_LOG_VERBOSE)
Browse files Browse the repository at this point in the history 
 - new parameter EKF2_LOG_VERBOSE to enable (currently enabled by default)
 - force advertise topics immediately (based on EKF2_LOG_VERBOSE and per aid source configuration)
 - logger optionally log all estimator topics at minimal rate
  • Loading branch information
@dagar
dagar authored Jun 27, 2024
1 parent 8070c70 commit 30b854d
Show file tree
Hide file tree
Showing 4 changed files with 172 additions and 179 deletions.
219 changes: 131 additions & 88 deletions src/modules/ekf2/EKF2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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<int32_t>(EvCtrl::VPOS)) {
_estimator_aid_src_ev_hgt_pub.advertise();
_estimator_ev_pos_bias_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS)) {
_estimator_aid_src_ev_hgt_pub.advertise();
_estimator_ev_pos_bias_pub.advertise();
}

if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::HPOS)) {
_estimator_aid_src_ev_pos_pub.advertise();
_estimator_ev_pos_bias_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::HPOS)) {
_estimator_aid_src_ev_pos_pub.advertise();
_estimator_ev_pos_bias_pub.advertise();
}

if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VEL)) {
_estimator_aid_src_ev_vel_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VEL)) {
_estimator_aid_src_ev_vel_pub.advertise();
}

if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
_estimator_aid_src_ev_yaw_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(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<int32_t>(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<int32_t>(GnssCtrl::VPOS)) {
_estimator_aid_src_gnss_hgt_pub.advertise();
_estimator_gnss_hgt_bias_pub.advertise();
}

if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::HPOS)) {
_estimator_aid_src_gnss_pos_pub.advertise();
_estimator_gps_status_pub.advertise();
}
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::HPOS)) {
_estimator_aid_src_gnss_pos_pub.advertise();
}

if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VEL)) {
_estimator_aid_src_gnss_vel_pub.advertise();
}
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VEL)) {
_estimator_aid_src_gnss_vel_pub.advertise();
}

# if defined(CONFIG_EKF2_GNSS_YAW)

if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
_estimator_aid_src_gnss_yaw_pub.advertise();
}
if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(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<int32_t>(ImuCtrl::GravityVector)) {
_estimator_aid_src_gravity_pub.advertise();
}
if (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::GravityVector)) {
_estimator_aid_src_gravity_pub.advertise();
}

#endif // CONFIG_EKF2_GRAVITY_FUSION

#if defined(CONFIG_EKF2_MAGNETOMETER)

if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
_estimator_aid_src_mag_pub.advertise();
}

#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_OPTICAL_FLOW)

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();
}
// 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 defined(CONFIG_EKF2_SIDESLIP)

#endif // CONFIG_EKF2_MAGNETOMETER
if (_param_ekf2_fuse_beta.get()) {
_estimator_aid_src_sideslip_pub.advertise();
}

_attitude_pub.advertise();
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
#endif // CONFIG_EKF2_SIDESLIP

#if defined(CONFIG_EKF2_WIND)
_wind_pub.advertise();
#endif // CONFIG_EKF2_WIND
} // 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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF2.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,6 +162,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem

void Run() override;

void AdvertiseTopics();
void VerifyParams();

void PublishAidSourceStatus(const hrt_abstime &timestamp);
Expand Down Expand Up @@ -482,6 +483,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)

DEFINE_PARAMETERS(
(ParamBool<px4::params::EKF2_LOG_VERBOSE>) _param_ekf2_log_verbose,
(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
Expand Down
5 changes: 5 additions & 0 deletions src/modules/ekf2/module.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1335,3 +1335,8 @@ parameters:
min: 1.0
unit: SD
decimal: 1
EKF2_LOG_VERBOSE:
description:
short: Verbose logging
type: boolean
default: 1
Loading

0 comments on commit 30b854d

Please sign in to comment.