mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 02:16:53 +08:00
ekf2: verbose logging control (new EKF2_LOG_VERBOSE)
- 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
This commit is contained in:
Daniel Agar
GitHub
+133
-90
@@ -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<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_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
|
||||
|
||||
@@ -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<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,
|
||||
|
||||
@@ -1335,3 +1335,8 @@ parameters:
|
||||
min: 1.0
|
||||
unit: SD
|
||||
decimal: 1
|
||||
EKF2_LOG_VERBOSE:
|
||||
description:
|
||||
short: Verbose logging
|
||||
type: boolean
|
||||
default: 1
|
||||
|
||||
@@ -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 */
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user