From 4ba84d56c9388f41e9ff366a7f4c73a2c4b5e4b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 15 Nov 2021 13:47:26 +0100 Subject: [PATCH] logger: introduce optional topics The current practice of adding topics to the default set isn't scalable, as it affects all setups. By making sure topics are advertised on init, logger can just discard topics that don't exist. This does not work for all topics, so topics are specifically marked as optional. It can be extended to more topics later on though. This reduces the list of topics by ~35 on a pixracer configured as quad, and reduces RAM usage by ~1KB. --- src/drivers/camera_capture/camera_capture.cpp | 1 + src/drivers/dshot/DShot.cpp | 2 + src/drivers/px4io/px4io.cpp | 2 + src/drivers/rpm/pcf8583/PCF8583.cpp | 2 + src/drivers/uavcan/actuators/esc.cpp | 1 + src/lib/mixer_module/mixer_module.cpp | 4 + src/lib/tecs/TECS.hpp | 2 - .../airspeed_selector_main.cpp | 1 + .../AngularVelocityController.cpp | 1 + .../camera_feedback/CameraFeedback.cpp | 2 + src/modules/ekf2/EKF2.cpp | 1 + .../FixedwingAttitudeControl.cpp | 2 + .../fw_autotune_attitude_control.cpp | 1 + .../FixedwingPositionControl.cpp | 2 + src/modules/gyro_fft/GyroFFT.cpp | 2 + src/modules/logger/logged_topics.cpp | 125 ++++++++++-------- src/modules/logger/logged_topics.h | 20 ++- .../mag_bias_estimator/MagBiasEstimator.cpp | 1 + src/modules/mavlink/mavlink_main.cpp | 1 + .../mc_autotune_attitude_control.cpp | 1 + .../MulticopterPositionControl.cpp | 1 + .../MulticopterRateControl.cpp | 1 + .../TemperatureCompensationModule.cpp | 2 + .../vtol_att_control_main.cpp | 2 + 24 files changed, 120 insertions(+), 60 deletions(-) diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 6d99627237..751302b56a 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -99,6 +99,7 @@ CameraCapture::CameraCapture() : } } + _trigger_pub.advertise(); } CameraCapture::~CameraCapture() diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 0764de6468..75da9b0a12 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -264,6 +264,8 @@ void DShot::init_telemetry(const char *device) } } + _telemetry->esc_status_pub.advertise(); + int ret = _telemetry->handler.init(device); if (ret != 0) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78fabd3df3..88df4b6717 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -538,6 +538,8 @@ int PX4IO::init() _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); } + _px4io_status_pub.advertise(); + update_params(); ScheduleNow(); diff --git a/src/drivers/rpm/pcf8583/PCF8583.cpp b/src/drivers/rpm/pcf8583/PCF8583.cpp index cbc8c701a4..bc5d54d19b 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.cpp +++ b/src/drivers/rpm/pcf8583/PCF8583.cpp @@ -57,6 +57,8 @@ int PCF8583::init() // start measurement resetCounter(); + _rpm_pub.advertise(); + ScheduleOnInterval(_param_pcf8583_pool.get()); return PX4_OK; diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index e4ee07b58f..5ce98b39c1 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -72,6 +72,7 @@ UavcanEscController::init() // ESC status will be relayed from UAVCAN bus into ORB at this rate _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + _esc_status_pub.advertise(); return res; } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45bd5c96c7..239faef6a9 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -113,6 +113,8 @@ _param_prefix(param_prefix) updateParams(); } + + _outputs_pub.advertise(); } MixingOutput::~MixingOutput() @@ -122,6 +124,8 @@ MixingOutput::~MixingOutput() px4_sem_destroy(&_lock); cleanupFunctions(); + + _outputs_pub.unadvertise(); } void MixingOutput::initParamHandles() diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index cfede513a5..f95f483423 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -206,8 +206,6 @@ private: static constexpr float _jerk_max = 1000.0f; // maximum jerk for creating height rate trajectories, we want infinite jerk so set a high value - uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication - enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; // timestamps diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 09716115a0..c03937dfe5 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -205,6 +205,7 @@ AirspeedModule::AirspeedModule(): update_params(); _perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed"); + _airspeed_validated_pub.advertise(); } AirspeedModule::~AirspeedModule() diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 0c06de2c90..8b0faf813b 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -50,6 +50,7 @@ AngularVelocityController::AngularVelocityController() : _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; parameters_updated(); + _rate_ctrl_status_pub.advertise(); } AngularVelocityController::~AngularVelocityController() diff --git a/src/modules/camera_feedback/CameraFeedback.cpp b/src/modules/camera_feedback/CameraFeedback.cpp index 93723df12e..f12c27e72e 100644 --- a/src/modules/camera_feedback/CameraFeedback.cpp +++ b/src/modules/camera_feedback/CameraFeedback.cpp @@ -47,6 +47,8 @@ CameraFeedback::init() return false; } + _capture_pub.advertise(); + return true; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d902ebf9e0..5ad3f734af 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -194,6 +194,7 @@ bool EKF2::multi_init(int imu, int mag) _ekf2_timestamps_pub.advertise(); _ekf_gps_drift_pub.advertise(); _estimator_baro_bias_pub.advertise(); + _estimator_event_flags_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 9580ccaed5..cc8789f4e7 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -65,6 +65,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : _pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get())); _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); + + _rate_ctrl_status_pub.advertise(); } FixedwingAttitudeControl::~FixedwingAttitudeControl() diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index d58835f8b2..9fbd31e024 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -47,6 +47,7 @@ FwAutotuneAttitudeControl::FwAutotuneAttitudeControl(bool is_vtol) : _actuator_controls_sub(this, is_vtol ? ORB_ID(actuator_controls_1) : ORB_ID(actuator_controls_0)), _actuator_controls_status_sub(is_vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)) { + _autotune_attitude_control_status_pub.advertise(); reset(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index f037da53ac..0a452b3aaf 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -70,6 +70,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : // limit to 50 Hz _local_pos_sub.set_interval_ms(20); + _tecs_status_pub.advertise(); + /* fetch initial parameter values */ parameters_update(); } diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index 32006d3338..1724505351 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/src/modules/gyro_fft/GyroFFT.cpp @@ -48,6 +48,8 @@ GyroFFT::GyroFFT() : _sensor_gyro_fft.peak_frequencies_y[i] = NAN; _sensor_gyro_fft.peak_frequencies_z[i] = NAN; } + + _sensor_gyro_fft_pub.advertise(); } GyroFFT::~GyroFFT() diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 93bc748dc7..e6b030d046 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -51,25 +51,26 @@ void LoggedTopics::add_default_topics() add_topic("actuator_controls_1", 100); add_topic("actuator_controls_2", 100); add_topic("actuator_controls_3", 100); - add_topic("actuator_controls_status_0", 300); + add_optional_topic("actuator_controls_status_0", 300); add_topic("airspeed", 1000); - add_topic("airspeed_validated", 200); - add_topic("autotune_attitude_control_status", 100); - add_topic("camera_capture"); - add_topic("camera_trigger"); + add_optional_topic("airspeed_validated", 200); + add_optional_topic("autotune_attitude_control_status", 100); + add_optional_topic("camera_capture"); + add_optional_topic("camera_trigger"); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("cpuload"); - add_topic("esc_status", 250); + add_optional_topic("esc_status", 250); add_topic("failure_detector_status", 100); add_topic("follow_target", 500); - add_topic("generator_status"); - add_topic("heater_status"); + add_optional_topic("generator_status"); + add_optional_topic("gps_dump"); + add_optional_topic("heater_status"); add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); - add_topic("internal_combustion_engine_status", 10); - add_topic("magnetometer_bias_estimate", 200); + add_optional_topic("internal_combustion_engine_status", 10); + add_optional_topic("magnetometer_bias_estimate", 200); add_topic("manual_control_setpoint", 200); add_topic("manual_control_switches"); add_topic("mission_result"); @@ -79,19 +80,19 @@ void LoggedTopics::add_default_topics() add_topic("parameter_update"); add_topic("position_controller_status", 500); add_topic("position_setpoint_triplet", 200); - add_topic("px4io_status"); + add_optional_topic("px4io_status"); add_topic("radio_status"); - add_topic("rpm", 500); + add_optional_topic("rpm", 500); add_topic("rtl_flight_time", 1000); add_topic("safety"); add_topic("sensor_combined"); - add_topic("sensor_correction"); - add_topic("sensor_gyro_fft", 50); + add_optional_topic("sensor_correction"); + add_optional_topic("sensor_gyro_fft", 50); add_topic("sensor_selection"); add_topic("sensors_status_imu", 200); add_topic("system_power", 500); - add_topic("takeoff_status", 1000); - add_topic("tecs_status", 200); + add_optional_topic("takeoff_status", 1000); + add_optional_topic("tecs_status", 200); add_topic("trajectory_setpoint", 200); add_topic("transponder_report"); add_topic("vehicle_acceleration", 50); @@ -112,15 +113,15 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_roi", 1000); add_topic("vehicle_status"); add_topic("vehicle_status_flags"); - add_topic("vtol_vehicle_status", 200); + add_optional_topic("vtol_vehicle_status", 200); add_topic("wind", 1000); // multi topics - add_topic_multi("actuator_outputs", 100, 3); - add_topic_multi("airspeed_wind", 1000); + add_optional_topic_multi("actuator_outputs", 100, 3); + add_topic_multi("airspeed_wind", 1000, 4); add_topic_multi("control_allocator_status", 200, 2); - add_topic_multi("rate_ctrl_status", 200, 2); - add_topic_multi("telemetry_status", 1000, 4); + add_optional_topic_multi("rate_ctrl_status", 200, 2); + add_optional_topic_multi("telemetry_status", 1000, 4); // EKF multi topics (currently max 9 estimators) #if CONSTRAINED_MEMORY @@ -128,39 +129,54 @@ void LoggedTopics::add_default_topics() #else static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed add_topic("estimator_selector_status"); - add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); + 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 - add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); + // always add the first instance + add_topic("ekf_gps_drift", 1000); + add_topic("estimator_baro_bias", 500); + add_topic("estimator_event_flags", 0); + 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("estimator_visual_odometry_aligned", 200); + add_topic("yaw_estimator_status", 1000); + + add_optional_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_event_flags", 0, 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("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); add_topic_multi("differential_pressure", 1000, 2); - add_topic_multi("distance_sensor", 1000, 2); + add_optional_topic_multi("distance_sensor", 1000, 2); add_topic_multi("optical_flow", 1000, 1); - add_topic_multi("sensor_accel", 1000, 4); - add_topic_multi("sensor_baro", 1000, 4); + add_optional_topic_multi("sensor_accel", 1000, 4); + add_optional_topic_multi("sensor_baro", 1000, 4); add_topic_multi("sensor_gps", 1000, 2); - add_topic_multi("sensor_gyro", 1000, 4); - add_topic_multi("sensor_mag", 1000, 4); + add_optional_topic_multi("sensor_gyro", 1000, 4); + add_optional_topic_multi("sensor_mag", 1000, 4); add_topic_multi("vehicle_imu", 500, 4); add_topic_multi("vehicle_imu_status", 1000, 4); - add_topic_multi("vehicle_magnetometer", 500, 4); + add_optional_topic_multi("vehicle_magnetometer", 500, 4); #ifdef CONFIG_ARCH_BOARD_PX4_SITL add_topic("actuator_controls_virtual_fw"); @@ -175,12 +191,6 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_local_position_groundtruth", 100); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ - int32_t gps_dump_comm = 0; - param_get(param_find("GPS_DUMP_COMM"), &gps_dump_comm); - - if (gps_dump_comm >= 1) { - add_topic("gps_dump"); - } int32_t sys_ctrl_alloc = 0; param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); @@ -362,13 +372,18 @@ void LoggedTopics::add_mission_topic(const char *name, uint16_t interval_ms) } } -bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, uint8_t instance) +bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, uint8_t instance, bool optional) { if (_subscriptions.count >= MAX_TOPICS_NUM) { PX4_WARN("Too many subscriptions, failed to add: %s %" PRIu8, topic->o_name, instance); return false; } + if (optional && orb_exists(topic, instance) != 0) { + PX4_DEBUG("Not adding non-existing optional topic %s %i", topic->o_name, instance); + return false; + } + RequestedSubscription &sub = _subscriptions.sub[_subscriptions.count++]; sub.interval_ms = interval_ms; sub.instance = instance; @@ -376,7 +391,7 @@ bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, ui return true; } -bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t instance) +bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t instance, bool optional) { const orb_metadata *const *topics = orb_get_topics(); bool success = false; @@ -401,7 +416,7 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins } if (!already_added) { - success = add_topic(topics[i], interval_ms, instance); + success = add_topic(topics[i], interval_ms, instance, optional); PX4_DEBUG("logging topic: %s(%" PRUu8 "), interval: %" PRUu16, topics[i]->o_name, instance, interval_ms); break; } @@ -411,11 +426,11 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins return success; } -bool LoggedTopics::add_topic_multi(const char *name, uint16_t interval_ms, uint8_t max_num_instances) +bool LoggedTopics::add_topic_multi(const char *name, uint16_t interval_ms, uint8_t max_num_instances, bool optional) { // add all possible instances for (uint8_t instance = 0; instance < max_num_instances; instance++) { - add_topic(name, interval_ms, instance); + add_topic(name, interval_ms, instance, optional); } return true; diff --git a/src/modules/logger/logged_topics.h b/src/modules/logger/logged_topics.h index 6cf4cfcd53..57f1eddb3b 100644 --- a/src/modules/logger/logged_topics.h +++ b/src/modules/logger/logged_topics.h @@ -107,9 +107,15 @@ private: * @param name topic name * @param interval limit in milliseconds if >0, otherwise log as fast as the topic is updated. * @param instance orb topic instance + * @param optional if true, the topic is only added if it exists * @return true on success */ - bool add_topic(const char *name, uint16_t interval_ms = 0, uint8_t instance = 0); + bool add_topic(const char *name, uint16_t interval_ms = 0, uint8_t instance = 0, bool optional = false); + + bool add_optional_topic(const char *name, uint16_t interval_ms = 0, uint8_t instance = 0) + { + return add_topic(name, interval_ms, instance, true); + } /** * Add a topic to be logged. @@ -117,9 +123,17 @@ private: * @param interval limit in milliseconds if >0, otherwise log as fast as the topic is updated. * @param instance orb topic instance * @param max_num_instances the max multi-instance to add. + * @param optional if true, the topic is only added if it exists * @return true on success */ - bool add_topic_multi(const char *name, uint16_t interval_ms = 0, uint8_t max_num_instances = ORB_MULTI_MAX_INSTANCES); + bool add_topic_multi(const char *name, uint16_t interval_ms = 0, uint8_t max_num_instances = ORB_MULTI_MAX_INSTANCES, + bool optional = false); + + bool add_optional_topic_multi(const char *name, uint16_t interval_ms = 0, + uint8_t max_num_instances = ORB_MULTI_MAX_INSTANCES) + { + return add_topic_multi(name, interval_ms, max_num_instances, true); + } /** * Parse a file containing a list of uORB topics to log, calling add_topic for each @@ -157,7 +171,7 @@ private: * add a logged topic (called by add_topic() above). * @return true on success */ - bool add_topic(const orb_metadata *topic, uint16_t interval_ms = 0, uint8_t instance = 0); + bool add_topic(const orb_metadata *topic, uint16_t interval_ms = 0, uint8_t instance = 0, bool optional = false); RequestedSubscriptionArray _subscriptions; int _num_mission_subs{0}; diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp index 15d05ba483..f8a52ea659 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -43,6 +43,7 @@ MagBiasEstimator::MagBiasEstimator() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { + _magnetometer_bias_estimate_pub.advertise(); } MagBiasEstimator::~MagBiasEstimator() diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 53edf1b89b..2721485f42 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -133,6 +133,7 @@ Mavlink::Mavlink() : } _event_sub.subscribe(); + _telemetry_status_pub.advertise(); } Mavlink::~Mavlink() diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index bf71b0b1e6..561ae136ad 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -45,6 +45,7 @@ McAutotuneAttitudeControl::McAutotuneAttitudeControl() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { + _autotune_attitude_control_status_pub.advertise(); reset(); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 0622f22250..5b3352c831 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -54,6 +54,7 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) : _failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND); _tilt_limit_slew_rate.setSlewRate(.2f); reset_setpoint_to_nan(_setpoint); + _takeoff_status_pub.advertise(); } MulticopterPositionControl::~MulticopterPositionControl() diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index c53a810998..c282099846 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -52,6 +52,7 @@ MulticopterRateControl::MulticopterRateControl(bool vtol) : _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; parameters_updated(); + _controller_status_pub.advertise(); } MulticopterRateControl::~MulticopterRateControl() diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index b7c21fe6a1..161ce9ef07 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -52,6 +52,8 @@ TemperatureCompensationModule::TemperatureCompensationModule() : _corrections.gyro_temperature[i] = NAN; _corrections.baro_temperature[i] = NAN; } + + _sensor_correction_pub.advertise(); } TemperatureCompensationModule::~TemperatureCompensationModule() diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 8e592a1ca3..ad17e7865d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -116,6 +116,8 @@ VtolAttitudeControl::VtolAttitudeControl() : } else { exit_and_cleanup(); } + + _vtol_vehicle_status_pub.advertise(); } VtolAttitudeControl::~VtolAttitudeControl()