cleanup logger default profile

- eliminate px4_sitl special handling
 - try to make as many topics optional as possible
 - ekf2 force initial advertisement based on configuration

src/drivers/adc/board_adc/ADC.cpp

@@ -45,6 +45,9 @@ ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) :
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
 	_base_address(base_address)
 {
+	_to_adc_report.advertise();
+	_to_system_power.advertise();
+
 	/* always enable the temperature sensor */
 	channels |= px4_arch_adc_temp_sensor_mask();
 

src/modules/angular_velocity_controller/AngularVelocityController.cpp

@@ -50,7 +50,11 @@ AngularVelocityController::AngularVelocityController() :
 	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 
 	parameters_updated();
+
 	_rate_ctrl_status_pub.advertise();
+	_vehicle_angular_acceleration_setpoint_pub.advertise();
+	_vehicle_thrust_setpoint_pub.advertise();
+	_vehicle_torque_setpoint_pub.advertise();
 }
 
 AngularVelocityController::~AngularVelocityController()

src/modules/commander/Commander.cpp

@@ -732,6 +732,14 @@ Commander::Commander() :
 	_failure_detector(this),
 	_health_and_arming_checks(this, _vehicle_status_flags, _vehicle_status)
 {
+	_actuator_armed_pub.advertise();
+	_commander_state_pub.advertise();
+	_failure_detector_status_pub.advertise();
+	_vehicle_control_mode_pub.advertise();
+	_vehicle_status_flags_pub.advertise();
+	_vehicle_status_pub.advertise();
+	_vehicle_command_ack_pub.advertise();
+
 	_vehicle_land_detected.landed = true;
 
 	_vehicle_status.system_id = 1;

src/modules/commander/HomePosition.cpp

@@ -40,6 +40,7 @@
 HomePosition::HomePosition(const vehicle_status_flags_s &vehicle_status_flags)
 	: _vehicle_status_flags(vehicle_status_flags)
 {
+	_home_position_pub.advertise();
 }
 
 bool HomePosition::hasMovedFromCurrentHomeLocation()

src/modules/ekf2/EKF2.cpp

@@ -168,18 +168,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
 	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
 {
-	// 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()
@@ -197,41 +186,121 @@ EKF2::~EKF2()
 	perf_free(_msg_missed_optical_flow_perf);
 }
 
-bool EKF2::multi_init(int imu, int mag)
+void EKF2::advertiseTopics()
 {
-	// advertise all topics to ensure consistent uORB instance numbering
-	_ekf2_timestamps_pub.advertise();
-	_estimator_baro_bias_pub.advertise();
-	_estimator_ev_hgt_bias_pub.advertise();
+	// advertise expected minimal topic set immediately to ensure logging
+	_attitude_pub.advertise();
+	_local_position_pub.advertise();
+
 	_estimator_event_flags_pub.advertise();
-	_estimator_gnss_hgt_bias_pub.advertise();
-	_estimator_gps_status_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
 	_estimator_innovations_pub.advertise();
-	_estimator_optical_flow_vel_pub.advertise();
-	_estimator_rng_hgt_bias_pub.advertise();
 	_estimator_sensor_bias_pub.advertise();
 	_estimator_states_pub.advertise();
-	_estimator_status_flags_pub.advertise();
 	_estimator_status_pub.advertise();
-	_estimator_visual_odometry_aligned_pub.advertise();
-	_yaw_est_pub.advertise();
+	_estimator_status_flags_pub.advertise();
 
-	_attitude_pub.advertise();
-	_local_position_pub.advertise();
-	_global_position_pub.advertise();
-	_odometry_pub.advertise();
-	_wind_pub.advertise();
+	if (_multi_mode) {
+		_global_position_pub.advertise();
+		_odometry_pub.advertise();
+		_wind_pub.advertise();
+		_ekf2_timestamps_pub.advertise();
+	}
+
+	// baro
+	if (_params->baro_ctrl != 0) {
+		_estimator_baro_bias_pub.advertise();
+		_estimator_aid_src_baro_hgt_pub.advertise();
+	}
+
+	// GNSS
+	if (_params->gnss_ctrl != 0) {
+
+		_estimator_gps_status_pub.advertise();
+
+		if (_params->gnss_ctrl & GnssCtrl::VEL) {
+			_yaw_est_pub.advertise();
+			_estimator_aid_src_gnss_vel_pub.advertise();
+		}
+
+		if (_params->gnss_ctrl & GnssCtrl::VPOS) {
+			_estimator_gnss_hgt_bias_pub.advertise();
+		}
+
+		if (_params->gnss_ctrl & GnssCtrl::HPOS) {
+			_global_position_pub.advertise();
+			_estimator_aid_src_gnss_pos_pub.advertise();
+		}
+
+		if (_params->gnss_ctrl & GnssCtrl::YAW) {
+			_estimator_aid_src_gnss_yaw_pub.advertise();
+		}
+	}
+
+	// range finder
+	if (_params->rng_ctrl != 0) {
+		_estimator_rng_hgt_bias_pub.advertise();
+		_estimator_aid_src_rng_hgt_pub.advertise();
+	}
+
+	// external vision (TODO: add and use EKF2_EV_CTRL parameter)
+	if ((_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)
+	    || (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW)
+	    || (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)
+	    || (_params->height_sensor_ref == HeightSensor::EV)) {
+
+		_estimator_ev_hgt_bias_pub.advertise();
+		_estimator_visual_odometry_aligned_pub.advertise();
+
+		if (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) {
+			//_estimator_aid_src_ev_pos_pub.advertise();
+		}
+
+		if (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) {
+			_estimator_aid_src_ev_yaw_pub.advertise();
+		}
+
+		if (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL) {
+			//_estimator_aid_src_ev_vel_pub.advertise();
+		}
+	}
+
+	// optical flow
+	if (_params->fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
+		_estimator_optical_flow_vel_pub.advertise();
+	}
+
+	// airspeed
+	if (_params->arsp_thr > 0.f) {
+		_estimator_aid_src_airspeed_pub.advertise();
+	}
+
+	// magnetometer
+	if (_params->mag_fusion_type != MagFuseType::NONE) {
+		_estimator_aid_src_mag_heading_pub.advertise();
+		_estimator_aid_src_mag_pub.advertise();
+	}
+}
+
+bool EKF2::multi_init(int imu, int mag)
+{
+	advertiseTopics();
 
 	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
 
 	const int status_instance = _estimator_states_pub.get_instance();
 
-	if ((status_instance >= 0) && changed_instance
+	if (changed_instance && (status_instance >= 0)
+	    && (_estimator_event_flags_pub.get_instance() == status_instance)
+	    && (_estimator_status_flags_pub.get_instance() == status_instance)
+	    && (_estimator_sensor_bias_pub.get_instance() == status_instance)
 	    && (_attitude_pub.get_instance() == status_instance)
 	    && (_local_position_pub.get_instance() == status_instance)
-	    && (_global_position_pub.get_instance() == status_instance)) {
+	    && (_global_position_pub.get_instance() == status_instance)
+	    && (_odometry_pub.get_instance() == status_instance)
+	    && (_wind_pub.get_instance() == status_instance)
+	   ) {
 
 		_instance = status_instance;
 

src/modules/ekf2/EKF2.hpp

@@ -133,6 +133,8 @@ private:
 	static constexpr uint8_t MAX_NUM_IMUS = 4;
 	static constexpr uint8_t MAX_NUM_MAGS = 4;
 
+	void advertiseTopics();
+
 	void Run() override;
 
 	void VerifyParams();

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -42,6 +42,10 @@ FlightModeManager::FlightModeManager() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_landing_gear_pub.advertise();
+	_trajectory_setpoint_pub.advertise();
+	_vehicle_constraints_pub.advertise();
+
 	updateParams();
 
 	// initialize all flight-tasks

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -48,6 +48,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
+	_actuator_controls_0_pub.advertise();
+	_actuator_controls_status_pub.advertise();
+	_attitude_sp_pub.advertise();
+	_rate_sp_pub.advertise();
+	_rate_ctrl_status_pub.advertise();
+	_vehicle_thrust_setpoint_pub.advertise();
+	_vehicle_torque_setpoint_pub.advertise();
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -57,7 +65,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	_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();
 	_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
 	_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
 }

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -65,6 +65,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 
 	_pos_ctrl_status_pub.advertise();
 	_pos_ctrl_landing_status_pub.advertise();
+	_npfg_status_pub.advertise();
 	_tecs_status_pub.advertise();
 
 	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);

src/modules/land_detector/LandDetector.cpp

@@ -49,6 +49,8 @@ LandDetector::LandDetector() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_vehicle_land_detected_pub.advertise();
+
 	_land_detected.ground_contact = true;
 	_land_detected.maybe_landed = true;
 	_land_detected.landed = true;

src/modules/load_mon/LoadMon.cpp

@@ -53,6 +53,7 @@ LoadMon::LoadMon() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
 {
+	_cpuload_pub.advertise();
 }
 
 LoadMon::~LoadMon()

src/modules/logger/logged_topics.cpp

@@ -37,6 +37,7 @@
 #include <parameters/param.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/px4_config.h>
+#include <lib/mathlib/mathlib.h>
 #include <uORB/topics/uORBTopics.hpp>
 
 #include <string.h>
@@ -45,236 +46,194 @@ using namespace px4::logger;
 
 void LoggedTopics::add_default_topics()
 {
-	add_topic("action_request");
-	add_topic("actuator_armed");
-	add_topic("actuator_controls_0", 50);
-	add_topic("actuator_controls_1", 100);
-	add_topic("actuator_controls_2", 100);
+	// default topics that may not be advertised immediately
+	add_topic("battery_status", 200);
+	add_topic("cellular_status", 200);
+	add_topic("differential_pressure", 1000);
+	add_topic("distance_sensor", 1000);
+	add_topic("gimbal_manager_set_attitude", 500);
+	add_topic("input_rc", 500);
+	add_topic("offboard_control_mode", 100);
+	add_topic("onboard_computer_status", 100);
+	add_topic("radio_status");
+	add_topic("sensor_accel", 1000);
+	add_topic("sensor_baro", 1000);
+	add_topic("sensor_gnss_relative", 1000);
+	add_topic("sensor_gps", 1000);
+	add_topic("sensor_gyro", 1000);
+	add_topic("sensor_hygrometer", 1000);
+	add_topic("sensor_mag", 1000);
+	add_topic("sensor_optical_flow", 1000);
+	add_topic("telemetry_status", 1000);
+	add_topic("transponder_report", 1000);
+	add_topic("vehicle_imu_status", 1000);
+	add_topic("vehicle_optical_flow", 500);
+	add_topic("wind", 1000);
+
+	// optional topics (must be advertised)
+	add_optional_topic("action_request");
+	add_optional_topic("actuator_armed");
+	add_optional_topic("actuator_controls_0", 50);
+	add_optional_topic("actuator_controls_1", 200);
+	add_optional_topic("actuator_controls_2", 200);
 	add_optional_topic("actuator_controls_status_0", 300);
-	add_topic("airspeed", 1000);
+	add_optional_topic("actuator_controls_virtual_fw", 300);
+	add_optional_topic("actuator_controls_virtual_mc", 300);
+	add_optional_topic("actuator_motors", 100);
+	add_optional_topic("actuator_servos", 100);
+	add_optional_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
-	add_optional_topic("autotune_attitude_control_status", 100);
+	add_optional_topic("autotune_attitude_control_status", 200);
 	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
-	add_topic("cellular_status", 200);
-	add_topic("commander_state");
-	add_topic("cpuload");
+	add_optional_topic("commander_state");
+	add_optional_topic("cpuload");
 	add_optional_topic("esc_status", 250);
-	add_topic("failure_detector_status", 100);
+	add_optional_topic("estimator_selector_status");
+	add_optional_topic("failure_detector_status", 100);
 	add_optional_topic("follow_target", 500);
 	add_optional_topic("follow_target_estimator", 200);
 	add_optional_topic("follow_target_status", 400);
-	add_topic("gimbal_manager_set_attitude", 500);
+	add_optional_topic("fw_virtual_attitude_setpoint", 500);
 	add_optional_topic("generator_status");
 	add_optional_topic("gps_dump");
 	add_optional_topic("gripper");
-	add_optional_topic("heater_status");
-	add_topic("home_position");
-	add_topic("hover_thrust_estimate", 100);
-	add_topic("input_rc", 500);
-	add_optional_topic("internal_combustion_engine_status", 10);
+	add_optional_topic("heater_status", 200);
+	add_optional_topic("home_position");
+	add_optional_topic("hover_thrust_estimate", 200);
+	add_optional_topic("internal_combustion_engine_status", 200);
 	add_optional_topic("irlock_report", 1000);
+	add_optional_topic("landing_gear", 1000);
 	add_optional_topic("landing_target_pose", 1000);
 	add_optional_topic("magnetometer_bias_estimate", 200);
-	add_topic("manual_control_setpoint", 200);
-	add_topic("manual_control_switches");
-	add_topic("mission_result");
-	add_topic("navigator_mission_item");
-	add_topic("npfg_status", 100);
-	add_topic("offboard_control_mode", 100);
-	add_topic("onboard_computer_status", 10);
-	add_topic("parameter_update");
-	add_topic("position_controller_status", 500);
-	add_topic("position_controller_landing_status", 100);
-	add_topic("position_setpoint_triplet", 200);
+	add_optional_topic("manual_control_setpoint", 200);
+	add_optional_topic("manual_control_switches");
+	add_optional_topic("mc_virtual_attitude_setpoint", 500);
+	add_optional_topic("mission_result");
+	add_optional_topic("navigator_mission_item");
+	add_optional_topic("npfg_status", 200);
+	add_optional_topic("parameter_update");
+	add_optional_topic("position_controller_landing_status", 200);
+	add_optional_topic("position_controller_status", 500);
+	add_optional_topic("position_setpoint_triplet", 200);
+	add_optional_topic("pps_capture");
 	add_optional_topic("px4io_status");
-	add_topic("radio_status");
-	add_topic("rtl_time_estimate", 1000);
-	add_topic("sensor_combined");
+	add_optional_topic("rate_ctrl_status", 200);
+	add_optional_topic("rtl_time_estimate", 1000);
+	add_optional_topic("sensor_combined");
 	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_optional_topic("sensor_selection");
+	add_optional_topic("sensors_status_imu", 200);
+	add_optional_topic("system_power", 500);
 	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);
-	add_topic("vehicle_air_data", 200);
-	add_topic("vehicle_angular_velocity", 20);
-	add_topic("vehicle_attitude", 50);
-	add_topic("vehicle_attitude_setpoint", 50);
-	add_topic("vehicle_command");
-	add_topic("vehicle_command_ack");
-	add_topic("vehicle_constraints", 1000);
-	add_topic("vehicle_control_mode");
-	add_topic("vehicle_global_position", 200);
-	add_topic("vehicle_gps_position", 500);
-	add_topic("vehicle_land_detected");
-	add_topic("vehicle_local_position", 100);
-	add_topic("vehicle_local_position_setpoint", 100);
-	add_topic("vehicle_magnetometer", 200);
-	add_topic("vehicle_rates_setpoint", 20);
-	add_topic("vehicle_roi", 1000);
-	add_topic("vehicle_status");
-	add_topic("vehicle_status_flags");
+	add_optional_topic("trajectory_setpoint", 200);
+	add_optional_topic("vehicle_acceleration", 50);
+	add_optional_topic("vehicle_air_data", 200);
+	add_optional_topic("vehicle_angular_acceleration", 20);
+	add_optional_topic("vehicle_angular_velocity", 20);
+	add_optional_topic("vehicle_attitude", 50);
+	add_optional_topic("vehicle_attitude_setpoint", 50);
+	add_optional_topic("vehicle_command");
+	add_optional_topic("vehicle_command_ack");
+	add_optional_topic("vehicle_constraints", 1000);
+	add_optional_topic("vehicle_control_mode");
+	add_optional_topic("vehicle_global_position", 200);
+	add_optional_topic("vehicle_gps_position", 500);
+	add_optional_topic("vehicle_land_detected");
+	add_optional_topic("vehicle_local_position", 100);
+	add_optional_topic("vehicle_local_position_setpoint", 100);
+	add_optional_topic("vehicle_optical_flow_vel", 100);
+	add_optional_topic("vehicle_rates_setpoint", 20);
+	add_optional_topic("vehicle_roi", 1000);
+	add_optional_topic("vehicle_status");
+	add_optional_topic("vehicle_status_flags");
 	add_optional_topic("vtol_vehicle_status", 200);
-	add_topic("wind", 1000);
+	add_optional_topic("yaw_estimator_status", 1000);
 
-	// multi topics
-	add_optional_topic_multi("actuator_outputs", 100, 3);
-	add_optional_topic_multi("airspeed_wind", 1000, 4);
-	add_optional_topic_multi("control_allocator_status", 200, 2);
-	add_optional_topic_multi("rate_ctrl_status", 200, 2);
-	add_optional_topic_multi("sensor_hygrometer", 500, 4);
-	add_optional_topic_multi("rpm", 200);
-	add_optional_topic_multi("telemetry_status", 1000, 4);
+	// EKF artificially limit multi-EKF logging to keep total topics a uint8_t
+	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 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("actuator_outputs", 100);
+	add_optional_topic_multi("airspeed_wind", 500);
+	add_optional_topic_multi("battery_status", 300);
+	add_optional_topic_multi("control_allocator_status", 200);
+	add_optional_topic_multi("differential_pressure", 500);
+	add_optional_topic_multi("distance_sensor", 500);
+	add_optional_topic_multi("estimator_aid_src_airspeed", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_aux_vel", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_baro_hgt", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_ev_yaw", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_fake_hgt", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_fake_pos", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_gnss_pos", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_gnss_vel", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_gnss_yaw", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_mag", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_mag_heading", 300, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_aid_src_rng_hgt", 300, 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
-
-	// always add the first instance
-	add_topic("estimator_baro_bias", 500);
-	add_topic("estimator_gnss_hgt_bias", 500);
-	add_topic("estimator_rng_hgt_bias", 500);
-	add_topic("estimator_ev_hgt_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("estimator_visual_odometry_aligned", 200);
-	add_topic("yaw_estimator_status", 1000);
-
 	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_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_ev_hgt_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_global_position", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_gps_status", 500, 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_local_position", 200, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_odometry", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_rng_hgt_bias", 500, 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_states", 500, 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);
+	add_optional_topic_multi("estimator_wind", 500, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("manual_control_input", 500);
+	add_optional_topic_multi("rate_ctrl_status", 200);
+	add_optional_topic_multi("rpm", 200);
+	add_optional_topic_multi("sensor_accel", 500);
+	add_optional_topic_multi("sensor_baro", 500);
+	add_optional_topic_multi("sensor_gnss_relative", 500);
+	add_optional_topic_multi("sensor_gps", 500);
+	add_optional_topic_multi("sensor_gyro", 500);
+	add_optional_topic_multi("sensor_hygrometer", 500);
+	add_optional_topic_multi("sensor_mag", 500);
+	add_optional_topic_multi("sensor_optical_flow", 500);
+	add_optional_topic_multi("telemetry_status", 500);
+	add_optional_topic_multi("vehicle_imu", 500);
+	add_optional_topic_multi("vehicle_imu_status", 500);
+	add_optional_topic_multi("vehicle_magnetometer", 200);
+	add_optional_topic_multi("vehicle_optical_flow", 500);
+	add_optional_topic_multi("vehicle_thrust_setpoint", 100);
+	add_optional_topic_multi("vehicle_torque_setpoint", 100);
+	add_optional_topic_multi("yaw_estimator_status", 500, 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_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_heading", 100, MAX_ESTIMATOR_INSTANCES);
-	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
-	// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, 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("sensor_accel", 1000, 4);
-	add_optional_topic_multi("sensor_baro", 1000, 4);
-	add_topic_multi("sensor_gps", 1000, 2);
-	add_topic_multi("sensor_gnss_relative", 1000, 1);
-	add_optional_topic_multi("sensor_gyro", 1000, 4);
-	add_optional_topic_multi("sensor_mag", 1000, 4);
-	add_optional_topic_multi("sensor_optical_flow", 1000, 2);
+	// simulation ground truth logging (SYS_HITL or PX4_SITL)
+#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
+	bool ground_truth_topics = true;
+#else
+	bool ground_truth_topics = false;
+#endif // CONFIG_ARCH_BOARD_PX4_SITL
 
-	add_topic_multi("vehicle_imu", 500, 4);
-	add_topic_multi("vehicle_imu_status", 1000, 4);
-	add_optional_topic_multi("vehicle_magnetometer", 500, 4);
-	add_optional_topic("vehicle_optical_flow", 500);
-	//add_optional_topic("vehicle_optical_flow_vel", 100);
-	add_optional_topic("pps_capture");
-
-	// additional control allocation logging
-	add_topic("actuator_motors", 100);
-	add_topic("actuator_servos", 100);
-	add_topic("vehicle_angular_acceleration", 20);
-	add_topic_multi("vehicle_thrust_setpoint", 20, 1);
-	add_topic_multi("vehicle_torque_setpoint", 20, 2);
-
-	// SYS_HITL: default ground truth logging for simulation
 	int32_t sys_hitl = 0;
 	param_get(param_find("SYS_HITL"), &sys_hitl);
 
 	if (sys_hitl >= 1) {
+		ground_truth_topics = true;
+	}
+
+	if (ground_truth_topics) {
 		add_topic("vehicle_angular_velocity_groundtruth", 10);
 		add_topic("vehicle_attitude_groundtruth", 10);
 		add_topic("vehicle_global_position_groundtruth", 100);
 		add_topic("vehicle_local_position_groundtruth", 20);
 	}
-
-#ifdef CONFIG_ARCH_BOARD_PX4_SITL
-	add_topic("actuator_controls_virtual_fw");
-	add_topic("actuator_controls_virtual_mc");
-	add_topic("fw_virtual_attitude_setpoint");
-	add_topic("mc_virtual_attitude_setpoint");
-	add_topic("time_offset");
-	add_topic("vehicle_angular_acceleration", 10);
-	add_topic("vehicle_angular_velocity", 10);
-	add_topic("vehicle_angular_velocity_groundtruth", 10);
-	add_topic("vehicle_attitude_groundtruth", 10);
-	add_topic("vehicle_global_position_groundtruth", 100);
-	add_topic("vehicle_local_position_groundtruth", 20);
-
-	// EKF replay
-	add_topic("estimator_baro_bias");
-	add_topic("estimator_gnss_hgt_bias");
-	add_topic("estimator_rng_hgt_bias");
-	add_topic("estimator_ev_hgt_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");
-	add_topic("estimator_visual_odometry_aligned");
-	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_gnss_yaw", 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_pos", 0, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
-
-#endif /* CONFIG_ARCH_BOARD_PX4_SITL */
 }
 
 void LoggedTopics::add_high_rate_topics()
@@ -313,16 +272,19 @@ void LoggedTopics::add_estimator_replay_topics()
 
 	// current EKF2 subscriptions
 	add_topic("airspeed");
-	add_topic("optical_flow");
+	add_topic("airspeed_validated");
+	add_topic("landing_target_pose");
 	add_topic("sensor_combined");
 	add_topic("sensor_selection");
 	add_topic("vehicle_air_data");
+	add_topic("vehicle_command");
 	add_topic("vehicle_gps_position");
 	add_topic("vehicle_land_detected");
 	add_topic("vehicle_magnetometer");
+	add_topic("vehicle_optical_flow");
 	add_topic("vehicle_status");
 	add_topic("vehicle_visual_odometry");
-	add_topic_multi("distance_sensor");
+	add_topic_multi("distance_sensor", 0, 3);
 }
 
 void LoggedTopics::add_thermal_calibration_topics()
@@ -492,7 +454,7 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins
 					PX4_DEBUG("logging topic %s(%" PRIu8 "), interval: %" PRIu16 ", already added, only setting interval",
 						  topics[i]->o_name, instance, interval_ms);
 
-					_subscriptions.sub[j].interval_ms = interval_ms;
+					_subscriptions.sub[j].interval_ms = math::min(interval_ms, _subscriptions.sub[j].interval_ms);
 					success = true;
 					already_added = true;
 					break;

src/modules/manual_control/ManualControl.cpp

@@ -42,6 +42,7 @@ ManualControl::ManualControl() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
+	_action_request_pub.advertise();
 }
 
 ManualControl::~ManualControl()

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -47,6 +47,7 @@ MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_hover_thrust_ekf_pub.advertise();
 	_valid_hysteresis.set_hysteresis_time_from(false, 2_s);
 	updateParams();
 	reset();

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -53,7 +53,11 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
 	parameters_update(true);
 	_tilt_limit_slew_rate.setSlewRate(.2f);
 	reset_setpoint_to_nan(_setpoint);
+
 	_takeoff_status_pub.advertise();
+	_vehicle_attitude_setpoint_pub.advertise();
+	_local_pos_sp_pub.advertise();
+
 }
 
 MulticopterPositionControl::~MulticopterPositionControl()

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -52,7 +52,13 @@ MulticopterRateControl::MulticopterRateControl(bool vtol) :
 	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 
 	parameters_updated();
+
+	_actuator_controls_0_pub.advertise();
+	_actuator_controls_status_0_pub.advertise();
 	_controller_status_pub.advertise();
+	_vehicle_rates_setpoint_pub.advertise();
+	_vehicle_thrust_setpoint_pub.advertise();
+	_vehicle_torque_setpoint_pub.advertise();
 }
 
 MulticopterRateControl::~MulticopterRateControl()

src/modules/navigator/mission.cpp

@@ -67,6 +67,7 @@ Mission::Mission(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
 {
+	_navigator_mission_item_pub.advertise();
 	mission_init();
 }
 

src/modules/navigator/navigator_main.cpp

@@ -79,6 +79,11 @@ Navigator::Navigator() :
 	_precland(this),
 	_rtl(this)
 {
+	_geofence_result_pub.advertise();
+	_pos_sp_triplet_pub.advertise();
+	_mission_result_pub.advertise();
+	_vehicle_roi_pub.advertise();
+
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;

src/modules/navigator/rtl.cpp

@@ -57,6 +57,7 @@ RTL::RTL(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
 {
+	_rtl_time_estimate_pub.advertise();
 	_param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP");
 	_param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN");
 	_param_mpc_land_speed = param_find("MPC_LAND_SPEED");

src/modules/rc_update/rc_update.cpp

@@ -67,6 +67,12 @@ RCUpdate::RCUpdate() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
+	if (_param_rc_chan_cnt.get() > 0) {
+		_rc_channels_pub.advertise();
+		_manual_control_input_pub.advertise();
+		_manual_control_switches_pub.advertise();
+	}
+
 	// initialize parameter handles
 	for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) {
 		char nbuf[16];