init adjustments to ensure used topics are advertised early (primaril…

…y 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
-	_attitude_pub.advertise();
-	_local_position_pub.advertise();
-	_global_position_pub.advertise();
-	_odometry_pub.advertise();
-	_wind_pub.advertise();
-
+	// advertise all topics to ensure consistent uORB instance numbering
 	_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(1_s);
+			ScheduleDelayed(10_ms);
 			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();
 
-	_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)
-					    && (vehicle_imu_sub.get().accel_device_id != 0)
-					    && (vehicle_imu_sub.get().gyro_device_id != 0)) {
+					if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
 
 						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
-									px4_usleep(10000);
+									_ekf2_selector.load()->ScheduleNow();
 
 								} 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
-						continue;
+						px4_usleep(1000); // give the sensors extra time to start
+						break;
 					}
 				}
 			}
 
 			if (multi_instances_allocated < multi_instances) {
-				px4_usleep(100000);
+				px4_usleep(10000);
 			}
 		}
 

src/modules/ekf2/EKF2.hpp

@@ -267,7 +267,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 
 	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 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 
 	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,19 +43,20 @@ 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()
 {
 	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 @@ class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem
 	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);