sensors: add SENS_IMU_MODE to optionally skip IMU voting/selection

src/modules/sensors/sensor_params.c

@@ -202,3 +202,15 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
  * @group Sensors
  */
 PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1);
+
+/**
+ * Sensors hub IMU mode
+ *
+ * @value 0 Disabled
+ * @value 1 Publish primary IMU selection
+ *
+ * @category system
+ * @reboot_required true
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_IMU_MODE, 1);

src/modules/sensors/sensors.cpp

@@ -209,7 +209,8 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
-		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
+		(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
+		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
 	)
 };
 
@@ -291,11 +292,8 @@ Sensors::~Sensors()
 
 bool Sensors::init()
 {
-	// initially run manually
-	ScheduleDelayed(10_ms);
-
 	_vehicle_imu_sub[0].registerCallback();
-
+	ScheduleNow();
 	return true;
 }
 
@@ -500,7 +498,11 @@ void Sensors::InitializeVehicleIMU()
 			gyro_sub.copy(&gyro);
 
 			if (accel.device_id > 0 && gyro.device_id > 0) {
-				VehicleIMU *imu = new VehicleIMU(i, i);
+				// 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 px4::wq_config_t &wq_config = px4::wq_configurations::nav_and_controllers;
+
+				VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config);
 
 				if (imu != nullptr) {
 					// Start VehicleIMU instance and store

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -45,11 +45,12 @@ using math::constrain;
 namespace sensors
 {
 
-VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
+VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	ScheduledWorkItem(MODULE_NAME, config),
 	_sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index),
-	_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index)
+	_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index),
+	_instance(instance)
 {
 	const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
 
@@ -82,6 +83,9 @@ VehicleIMU::~VehicleIMU()
 	perf_free(_accel_update_perf);
 	perf_free(_gyro_generation_gap_perf);
 	perf_free(_gyro_update_perf);
+
+	_vehicle_imu_pub.unadvertise();
+	_vehicle_imu_status_pub.unadvertise();
 }
 
 bool VehicleIMU::Start()
@@ -89,7 +93,10 @@ bool VehicleIMU::Start()
 	// force initial updates
 	ParametersUpdate(true);
 
-	return _sensor_gyro_sub.registerCallback() && _sensor_accel_sub.registerCallback();
+	_sensor_gyro_sub.registerCallback();
+	_sensor_accel_sub.registerCallback();
+	ScheduleNow();
+	return true;
 }
 
 void VehicleIMU::Stop()
@@ -448,11 +455,12 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
 void VehicleIMU::PrintStatus()
 {
 	if (_accel_calibration.device_id() == _gyro_calibration.device_id()) {
-		PX4_INFO("IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _accel_calibration.device_id(),
+		PX4_INFO("%d - IMU ID: %d, accel interval: %.1f us, gyro interval: %.1f us", _instance, _accel_calibration.device_id(),
 			 (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
 
 	} else {
-		PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _accel_calibration.device_id(),
+		PX4_INFO("%d - Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us", _instance,
+			 _accel_calibration.device_id(),
 			 (double)_accel_interval.update_interval, _gyro_calibration.device_id(), (double)_gyro_interval.update_interval);
 	}
 

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -60,7 +60,7 @@ class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	VehicleIMU() = delete;
-	VehicleIMU(uint8_t accel_index = 0, uint8_t gyro_index = 0);
+	VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config);
 
 	~VehicleIMU() override;
 
@@ -116,6 +116,8 @@ private:
 
 	bool _intervals_configured{false};
 
+	const uint8_t _instance;
+
 	perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")};
 	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
 	perf_counter_t _gyro_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro update interval")};

src/modules/sensors/voted_sensors_update.cpp

@@ -137,7 +137,7 @@ void VotedSensorsUpdate::parametersUpdate()
 
 void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 {
-	for (int uorb_index = 0; uorb_index < 3; uorb_index++) {
+	for (int uorb_index = 0; uorb_index < SENSOR_COUNT_MAX; uorb_index++) {
 		vehicle_imu_s imu_report;
 
 		if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0)
@@ -182,14 +182,41 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	}
 
 	// find the best sensor
-	int accel_best_index;
-	int gyro_best_index;
+	int accel_best_index = -1;
+	int gyro_best_index = -1;
+
+	if (_param_sens_imu_mode.get()) {
 		_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
 		_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
 
 		checkFailover(_accel, "Accel");
 		checkFailover(_gyro, "Gyro");
 
+	} else {
+
+		// use sensor_selection to find best
+		if (_sensor_selection_sub.update(&_selection)) {
+			// reset inconsistency checks against primary
+			for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
+				_accel_diff[sensor_index].zero();
+			}
+
+			for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
+				_gyro_diff[sensor_index].zero();
+			}
+		}
+
+		for (int i = 0; i < SENSOR_COUNT_MAX; i++) {
+			if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) {
+				accel_best_index = i;
+			}
+
+			if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) {
+				gyro_best_index = i;
+			}
+		}
+	}
+
 	// write data for the best sensor to output variables
 	if ((accel_best_index >= 0) && (gyro_best_index >= 0)) {
 		raw.timestamp = _last_sensor_data[gyro_best_index].timestamp;
@@ -323,12 +350,14 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 	imuPoll(raw);
 
 	// publish sensor selection if changed
+	if (_param_sens_imu_mode.get()) {
 		if (_selection_changed) {
 			// don't publish until selected IDs are valid
 			if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) {
 				_selection.timestamp = hrt_absolute_time();
 				_sensor_selection_pub.publish(_selection);
 				_selection_changed = false;
+			}
 
 			for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
 				_accel_diff[sensor_index].zero();

src/modules/sensors/voted_sensors_update.h

@@ -167,9 +167,11 @@ private:
 	uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)};	/**< handle to the sensor selection uORB topic */
 	uORB::Publication<sensors_status_imu_s> _sensors_status_imu_pub{ORB_ID(sensors_status_imu)};
 
-	uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3];
+	uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[SENSOR_COUNT_MAX];
 	uORB::SubscriptionMultiArray<vehicle_imu_status_s, ACCEL_COUNT_MAX> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
 
+	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
+
 	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {};	/**< latest sensor data from all sensors instances */
 
 	const bool _hil_enabled{false};			/**< is hardware-in-the-loop mode enabled? */
@@ -185,6 +187,10 @@ private:
 	uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {};	/**< latest full timestamp */
 
 	sensor_selection_s _selection {};		/**< struct containing the sensor selection to be published to the uORB */
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
+	)
 };
 
 } /* namespace sensors */