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)

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();
-	_vehicle_angular_velocity.Start();
+	parameters_update();
+
+	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,
-							sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_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) / 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;
-
-	return 0;
+	parametersUpdate();
 }
 
 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.
 	 */