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

@@ -99,6 +99,7 @@ CameraCapture::CameraCapture() :
 		}
 	}
 
+	_trigger_pub.advertise();
 }
 
 CameraCapture::~CameraCapture()

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) {

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();

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;

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;
 }

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()

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_s>	_tecs_status_pub{ORB_ID(tecs_status)};	///< TECS status publication
-
 	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};
 
 	// timestamps

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()

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()

src/modules/camera_feedback/CameraFeedback.cpp

@@ -47,6 +47,8 @@ CameraFeedback::init()
 		return false;
 	}
 
+	_capture_pub.advertise();
+
 	return true;
 }
 

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();

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()

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

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

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()

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;

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};

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()

src/modules/mavlink/mavlink_main.cpp

@@ -133,6 +133,7 @@ Mavlink::Mavlink() :
 	}
 
 	_event_sub.subscribe();
+	_telemetry_status_pub.advertise();
 }
 
 Mavlink::~Mavlink()

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

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()

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()

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()

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()