init adjustments to ensure used topics are advertised early (primarily for logging)

- multi-EKF create each instance as soon as IMU & mag are advertised (before device id populated)

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -223,6 +223,13 @@ rc_update start
 manual_control start
 sensors start
 commander start
+
+# Configure vehicle type specific parameters.
+# Note: rc.vehicle_setup is the entry point for rc.interface,
+#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
+#
+. ${R}etc/init.d/rc.vehicle_setup
+
 navigator start
 
 # Try to start the micrortps_client with UDP transport if module exists
@@ -252,12 +259,6 @@ then
 	gyro_calibration start
 fi
 
-# Configure vehicle type specific parameters.
-# Note: rc.vehicle_setup is the entry point for rc.interface,
-#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
-#
-. ${R}etc/init.d/rc.vehicle_setup
-
 #user defined mavlink streams for instances can be in PATH
 . px4-rc.mavlink
 

ROMFS/px4fmu_common/init.d/rcS

@@ -408,6 +408,13 @@ else
 		commander start
 	fi
 
+	#
+	# Configure vehicle type specific parameters.
+	# Note: rc.vehicle_setup is the entry point for rc.interface,
+	#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
+	#
+	. ${R}etc/init.d/rc.vehicle_setup
+
 	# Pre-takeoff continuous magnetometer calibration
 	if param compare -s MBE_ENABLE 1
 	then
@@ -458,13 +465,6 @@ else
 		fi
 	fi
 
-	#
-	# Configure vehicle type specific parameters.
-	# Note: rc.vehicle_setup is the entry point for rc.interface,
-	#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
-	#
-	. ${R}etc/init.d/rc.vehicle_setup
-
 	#
 	# Play the startup tune (if not disabled or there is an error)
 	#

src/drivers/heater/heater.cpp

@@ -73,6 +73,8 @@ Heater::Heater() :
 	}
 
 #endif
+
+	_heater_status_pub.advertise();
 }
 
 Heater::~Heater()

src/lib/mixer_module/mixer_module.cpp

@@ -103,6 +103,9 @@ _param_prefix(param_prefix)
 		}
 
 		updateParams();
+
+	} else {
+		_control_allocator_status_pub.advertise();
 	}
 
 	_outputs_pub.advertise();

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -206,6 +206,8 @@ AirspeedModule::AirspeedModule():
 
 	_perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed");
 	_airspeed_validated_pub.advertise();
+	_wind_est_pub[0].advertise();
+	_wind_est_pub[1].advertise();
 }
 
 AirspeedModule::~AirspeedModule()

src/modules/control_allocator/ControlAllocator.cpp

@@ -54,6 +54,11 @@ ControlAllocator::ControlAllocator() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
+	_control_allocator_status_pub.advertise();
+	_actuator_motors_pub.advertise();
+	_actuator_servos_pub.advertise();
+	_actuator_servos_trim_pub.advertise();
+
 	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
 		char buffer[17];
 		snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);

src/modules/ekf2/CMakeLists.txt

@@ -42,6 +42,8 @@ px4_add_module(
 		#-DDEBUG_BUILD
 	INCLUDES
 		EKF
+	PRIORITY
+		"SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads
 	STACK_MAX
 		3600
 	SRCS

src/modules/ekf2/EKF2.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -164,6 +164,18 @@ 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();
 }
 
 EKF2::~EKF2()
@@ -183,13 +195,7 @@ EKF2::~EKF2()
 
 bool EKF2::multi_init(int imu, int mag)
 {
-	// advertise immediately to ensure consistent uORB instance numbering
+	// advertise all topics to ensure consistent uORB instance numbering
-	_attitude_pub.advertise();
-	_local_position_pub.advertise();
-	_global_position_pub.advertise();
-	_odometry_pub.advertise();
-	_wind_pub.advertise();
-
 	_ekf2_timestamps_pub.advertise();
 	_estimator_baro_bias_pub.advertise();
 	_estimator_event_flags_pub.advertise();
@@ -205,6 +211,12 @@ bool EKF2::multi_init(int imu, int mag)
 	_estimator_visual_odometry_aligned_pub.advertise();
 	_yaw_est_pub.advertise();
 
+	_attitude_pub.advertise();
+	_local_position_pub.advertise();
+	_global_position_pub.advertise();
+	_odometry_pub.advertise();
+	_wind_pub.advertise();
+
 	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
 
 	const int status_instance = _estimator_states_pub.get_instance();
@@ -320,8 +332,7 @@ void EKF2::Run()
 		}
 
 		if (!_callback_registered) {
-			PX4_WARN("%d - failed to register callback, retrying", _instance);
+			ScheduleDelayed(10_ms);
-			ScheduleDelayed(1_s);
 			return;
 		}
 	}
@@ -593,6 +604,9 @@ void EKF2::Run()
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
+
+	// re-schedule as backup timeout
+	ScheduleDelayed(100_ms);
 }
 
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -692,11 +706,19 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 		event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped;
 
 		event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-		_estimator_event_flags_pub.publish(event_flags);
+		_estimator_event_flags_pub.update(event_flags);
-	}
+
+		_last_event_flags_publish = event_flags.timestamp;
 
 		_ekf.clear_information_events();
 		_ekf.clear_warning_events();
+
+	} else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) {
+		// continue publishing periodically
+		_estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		_estimator_event_flags_pub.update();
+		_last_event_flags_publish = _estimator_event_flags_pub.get().timestamp;
+	}
 }
 
 void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
@@ -1198,7 +1220,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 {
 	// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
-	bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s);
+	bool update = (timestamp >= _last_status_flags_publish + 1_s);
 
 	// filter control status
 	if (_ekf.control_status().value != _filter_control_status) {
@@ -1296,7 +1318,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
 
-		_last_status_flag_update = status_flags.timestamp;
+		_last_status_flags_publish = status_flags.timestamp;
 	}
 }
 
@@ -2027,9 +2049,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 					vehicle_mag_sub.update();
 
 					// Mag & IMU data must be valid, first mag can be ignored initially
-					if ((vehicle_mag_sub.get().device_id != 0 || mag == 0)
+					if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
-					    && (vehicle_imu_sub.get().accel_device_id != 0)
-					    && (vehicle_imu_sub.get().gyro_device_id != 0)) {
 
 						if (!ekf2_instance_created[imu][mag]) {
 							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
@@ -2043,17 +2063,11 @@ int EKF2::task_spawn(int argc, char *argv[])
 									multi_instances_allocated++;
 									ekf2_instance_created[imu][mag] = true;
 
-									if (actual_instance == 0) {
-										// force selector to run immediately if first instance started
-										_ekf2_selector.load()->ScheduleNow();
-									}
-
 									PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
 										  imu, vehicle_imu_sub.get().accel_device_id,
 										  mag, vehicle_mag_sub.get().device_id);
 
-									// sleep briefly before starting more instances
+									_ekf2_selector.load()->ScheduleNow();
-									px4_usleep(10000);
 
 								} else {
 									PX4_ERR("instance numbering problem instance: %d", actual_instance);
@@ -2063,20 +2077,20 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 							} else {
 								PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
-								px4_usleep(1000000);
+								px4_usleep(100000);
 								break;
 							}
 						}
 
 					} else {
-						px4_usleep(50000); // give the sensors extra time to start
+						px4_usleep(1000); // give the sensors extra time to start
-						continue;
+						break;
 					}
 				}
 			}
 
 			if (multi_instances_allocated < multi_instances) {
-				px4_usleep(100000);
+				px4_usleep(10000);
 			}
 		}
 

src/modules/ekf2/EKF2.hpp

@@ -267,7 +267,9 @@ private:
 
 	bool _callback_registered{false};
 
-	hrt_abstime _last_status_flag_update{0};
+	hrt_abstime _last_event_flags_publish{0};
+	hrt_abstime _last_status_flags_publish{0};
+
 	hrt_abstime _last_range_sensor_update{0};
 
 	uint32_t _filter_control_status{0};
@@ -282,7 +284,7 @@ private:
 
 	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::PublicationMulti<estimator_baro_bias_s>        _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
-	uORB::PublicationMulti<estimator_event_flags_s>      _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
+	uORB::PublicationMultiData<estimator_event_flags_s>  _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
 	uORB::PublicationMulti<estimator_gps_status_s>       _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};

src/modules/ekf2/EKF2Selector.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,6 +43,13 @@ EKF2Selector::EKF2Selector() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers)
 {
+	_estimator_selector_status_pub.advertise();
+	_sensor_selection_pub.advertise();
+	_vehicle_attitude_pub.advertise();
+	_vehicle_global_position_pub.advertise();
+	_vehicle_local_position_pub.advertise();
+	_vehicle_odometry_pub.advertise();
+	_wind_pub.advertise();
 }
 
 EKF2Selector::~EKF2Selector()
@@ -50,12 +57,6 @@ EKF2Selector::~EKF2Selector()
 	Stop();
 }
 
-bool EKF2Selector::Start()
-{
-	ScheduleNow();
-	return true;
-}
-
 void EKF2Selector::Stop()
 {
 	for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
@@ -673,9 +674,6 @@ void EKF2Selector::PublishWindEstimate()
 
 void EKF2Selector::Run()
 {
-	// re-schedule as backup timeout
-	ScheduleDelayed(FILTER_UPDATE_PERIOD);
-
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -703,6 +701,7 @@ void EKF2Selector::Run()
 
 		// if still invalid return early and check again on next scheduled run
 		if (_selected_instance == INVALID_INSTANCE) {
+			ScheduleDelayed(100_ms);
 			return;
 		}
 	}
@@ -803,6 +802,9 @@ void EKF2Selector::Run()
 	PublishVehicleGlobalPosition();
 	PublishVehicleOdometry();
 	PublishWindEstimate();
+
+	// re-schedule as backup timeout
+	ScheduleDelayed(FILTER_UPDATE_PERIOD);
 }
 
 void EKF2Selector::PublishEstimatorSelectorStatus()

src/modules/ekf2/EKF2Selector.hpp

@@ -70,7 +70,6 @@ public:
 	EKF2Selector();
 	~EKF2Selector() override;
 
-	bool Start();
 	void Stop();
 
 	void PrintStatus();

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);
 
+	_pos_ctrl_status_pub.advertise();
+	_pos_ctrl_landing_status_pub.advertise();
 	_tecs_status_pub.advertise();
 
 	_slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE);

src/modules/logger/logged_topics.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -120,10 +120,10 @@ void LoggedTopics::add_default_topics()
 
 	// multi topics
 	add_optional_topic_multi("actuator_outputs", 100, 3);
-	add_topic_multi("airspeed_wind", 1000, 4);
+	add_optional_topic_multi("airspeed_wind", 1000, 4);
-	add_topic_multi("control_allocator_status", 200, 2);
+	add_optional_topic_multi("control_allocator_status", 200, 2);
 	add_optional_topic_multi("rate_ctrl_status", 200, 2);
-	add_topic_multi("sensor_hygrometer", 500, 4);
+	add_optional_topic_multi("sensor_hygrometer", 500, 4);
 	add_optional_topic_multi("rpm", 200);
 	add_optional_topic_multi("telemetry_status", 1000, 4);
 
@@ -132,7 +132,7 @@ void LoggedTopics::add_default_topics()
 	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1;
 #else
 	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed
-	add_topic("estimator_selector_status");
+	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);
@@ -440,7 +440,7 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins
 				if (_subscriptions.sub[j].id == static_cast<ORB_ID>(topics[i]->o_id) &&
 				    _subscriptions.sub[j].instance == instance) {
 
-					PX4_DEBUG("logging topic %s(%" PRUu8 "), interval: %" PRUu16 ", already added, only setting interval",
+					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;
@@ -452,7 +452,11 @@ 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, optional);
-				PX4_DEBUG("logging topic: %s(%" PRUu8 "), interval: %" PRUu16, topics[i]->o_name, instance, interval_ms);
+
+				if (success) {
+					PX4_DEBUG("logging topic: %s(%" PRIu8 "), interval: %" PRIu16, topics[i]->o_name, instance, interval_ms);
+				}
+
 				break;
 			}
 		}

src/modules/sensors/sensors.cpp

@@ -233,6 +233,11 @@ Sensors::Sensors(bool hil_enabled) :
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
 	_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
 {
+	_sensor_pub.advertise();
+
+	_vehicle_angular_velocity.Start();
+	_vehicle_acceleration.Start();
+
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
@@ -250,11 +255,17 @@ Sensors::Sensors(bool hil_enabled) :
 	param_find("SYS_CAL_TMAX");
 	param_find("SYS_CAL_TMIN");
 
+	_sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
+
 	_airspeed_validator.set_timeout(300000);
 	_airspeed_validator.set_equal_value_threshold(100);
 
-	_vehicle_acceleration.Start();
+	parameters_update();
-	_vehicle_angular_velocity.Start();
+
+	InitializeVehicleAirData();
+	InitializeVehicleGPSPosition();
+	InitializeVehicleIMU();
+	InitializeVehicleMagnetometer();
 }
 
 Sensors::~Sensors()
@@ -369,6 +380,10 @@ int Sensors::parameters_update()
 		}
 	}
 
+	InitializeVehicleAirData();
+	InitializeVehicleGPSPosition();
+	InitializeVehicleMagnetometer();
+
 	return PX4_OK;
 }
 
@@ -569,14 +584,9 @@ void Sensors::InitializeVehicleIMU()
 		if (_vehicle_imu_list[i] == nullptr) {
 
 			uORB::Subscription accel_sub{ORB_ID(sensor_accel), i};
-			sensor_accel_s accel{};
-			accel_sub.copy(&accel);
-
 			uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i};
-			sensor_gyro_s gyro{};
-			gyro_sub.copy(&gyro);
 
-			if (accel.device_id > 0 && gyro.device_id > 0) {
+			if (accel_sub.advertised() && gyro_sub.advertised()) {
 				// if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ
 				//   otherwise each VehicleIMU runs in a corresponding INSx WQ
 				const bool multi_mode = (_param_sens_imu_mode.get() == 0);
@@ -627,21 +637,8 @@ void Sensors::Run()
 		return;
 	}
 
-	// run once
-	if (_last_config_update == 0) {
-		InitializeVehicleAirData();
-		InitializeVehicleIMU();
-		InitializeVehicleGPSPosition();
-		InitializeVehicleMagnetometer();
-		_voted_sensors_update.init(_sensor_combined);
-		parameter_update_poll(true);
-	}
-
 	perf_begin(_loop_perf);
 
-	// backup schedule as a watchdog timeout
-	ScheduleDelayed(10_ms);
-
 	// check vehicle status for changes to publication state
 	if (_vcontrol_mode_sub.updated()) {
 		vehicle_control_mode_s vcontrol_mode{};
@@ -651,23 +648,9 @@ void Sensors::Run()
 		}
 	}
 
-	_voted_sensors_update.sensorsPoll(_sensor_combined);
-
-	// check analog airspeed
-	adc_poll();
-
-	diff_pres_poll();
-
-	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
-
-		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
-		_sensor_pub.publish(_sensor_combined);
-		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
-	}
-
 	// keep adding sensors as long as we are not armed,
 	// when not adding sensors poll for param updates
-	if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) {
+	if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) {
 
 		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
 		const int n_baro  = orb_group_count(ORB_ID(sensor_baro));
@@ -683,10 +666,6 @@ void Sensors::Run()
 			_n_mag = n_mag;
 
 			parameters_update();
-
-			InitializeVehicleAirData();
-			InitializeVehicleGPSPosition();
-			InitializeVehicleMagnetometer();
 		}
 
 		// sensor device id (not just orb_group_count) must be populated before IMU init can succeed
@@ -700,6 +679,23 @@ void Sensors::Run()
 		parameter_update_poll();
 	}
 
+	_voted_sensors_update.sensorsPoll(_sensor_combined);
+
+	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
+
+		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
+		_sensor_pub.publish(_sensor_combined);
+		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
+	}
+
+	// check analog airspeed
+	adc_poll();
+
+	diff_pres_poll();
+
+	// backup schedule as a watchdog timeout
+	ScheduleDelayed(10_ms);
+
 	perf_end(_loop_perf);
 }
 

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,6 +46,8 @@ VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_vehicle_acceleration_pub.advertise();
+
 	CheckAndUpdateFilters();
 }
 

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -48,6 +48,8 @@ VehicleAirData::VehicleAirData() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_vehicle_air_data_pub.advertise();
+
 	_voter.set_timeout(SENSOR_TIMEOUT);
 }
 
@@ -334,7 +336,7 @@ void VehicleAirData::Run()
 	}
 
 	// reschedule timeout
-	ScheduleDelayed(100_ms);
+	ScheduleDelayed(50_ms);
 
 	perf_end(_cycle_perf);
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -46,6 +46,8 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
+	_vehicle_angular_acceleration_pub.advertise();
+	_vehicle_angular_velocity_pub.advertise();
 }
 
 VehicleAngularVelocity::~VehicleAngularVelocity()

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,6 +43,7 @@ VehicleGPSPosition::VehicleGPSPosition() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_vehicle_gps_position_pub.advertise();
 }
 
 VehicleGPSPosition::~VehicleGPSPosition()
@@ -103,9 +104,6 @@ void VehicleGPSPosition::Run()
 	perf_begin(_cycle_perf);
 	ParametersUpdate();
 
-	// GPS blending
-	ScheduleDelayed(500_ms); // backup schedule
-
 	// Check all GPS instance
 	bool any_gps_updated = false;
 	bool gps_updated = false;
@@ -135,6 +133,8 @@ void VehicleGPSPosition::Run()
 		}
 	}
 
+	ScheduleDelayed(300_ms); // backup schedule
+
 	perf_end(_cycle_perf);
 }
 

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -650,8 +650,8 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		_gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us));
 		_gyro_integrator.set_reset_samples(gyro_integral_samples);
 
-		_backup_schedule_timeout_us = math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us,
+		_backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us,
-							sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us);
+					      sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us) / 2, 1000, 20000);
 
 		// gyro: find largest integer multiple of gyro_integral_samples
 		for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) {

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -36,6 +36,7 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/events.h>
 #include <lib/geo/geo.h>
+#include <lib/sensor_calibration/Utilities.hpp>
 
 namespace sensors
 {
@@ -55,6 +56,20 @@ VehicleMagnetometer::VehicleMagnetometer() :
 	_voter.set_equal_value_threshold(1000);
 
 	ParametersUpdate(true);
+
+	_vehicle_magnetometer_pub[0].advertise();
+	_sensor_preflight_mag_pub.advertise();
+
+	// if publishing multiple mags advertise instances immediately for existing calibrations
+	if (!_param_sens_mag_mode.get()) {
+		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+			uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i);
+
+			if (device_id_mag != 0) {
+				_vehicle_magnetometer_pub[i].advertise();
+			}
+		}
+	}
 }
 
 VehicleMagnetometer::~VehicleMagnetometer()
@@ -547,7 +562,7 @@ void VehicleMagnetometer::Run()
 	UpdateMagCalibration();
 
 	// reschedule timeout
-	ScheduleDelayed(40_ms);
+	ScheduleDelayed(50_ms);
 
 	perf_end(_cycle_perf);
 }

src/modules/sensors/voted_sensors_update.cpp

@@ -54,22 +54,17 @@ VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled,
 	_vehicle_imu_sub(vehicle_imu_sub),
 	_hil_enabled(hil_enabled)
 {
+	_sensor_selection_pub.advertise();
+	_sensors_status_imu_pub.advertise();
+
 	if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit
 		_gyro.voter.set_timeout(500000);
 		_accel.voter.set_timeout(500000);
 	}
-}
-
-int VotedSensorsUpdate::init(sensor_combined_s &raw)
-{
-	raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
-	raw.timestamp = 0;
 
 	initializeSensors();
 
-	_selection_changed = true;
+	parametersUpdate();
-
-	return 0;
 }
 
 void VotedSensorsUpdate::initializeSensors()

src/modules/sensors/voted_sensors_update.h

@@ -78,12 +78,6 @@ public:
 	 */
 	VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]);
 
-	/**
-	 * initialize subscriptions etc.
-	 * @return 0 on success, <0 otherwise
-	 */
-	int init(sensor_combined_s &raw);
-
 	/**
 	 * This tries to find new sensor instances. This is called from init(), then it can be called periodically.
 	 */