sensors: move to WQ

Running the sensors module out of the same WQ thread as the estimator, position, and attitude controllers is a bit safer and prevents potential priority and starvation issues. There is a very small increase in latency (~50 us) between sensors and ekf2 execution (on average). This also saves a little bit of memory (~ 3 kB) and cpu (~1-1.5% depending on the board).

platforms/common/include/px4_platform_common/tasks.h

@@ -124,11 +124,6 @@ typedef struct {
 // wait for the sensor hub if its data is coming from it.
 #define SCHED_PRIORITY_ESTIMATOR		(PX4_WQ_HP_BASE - 5)
 
-// The sensor hub conditions sensor data. It is not the fastest component
-// in the controller chain, but provides easy-to-use data to the more
-// complex downstream consumers
-#define SCHED_PRIORITY_SENSOR_HUB		(PX4_WQ_HP_BASE - 6)
-
 // Position controllers typically are in a blocking wait on estimator data
 // so when new sensor data is available they will run last. Keeping them
 // on a high priority ensures that they are the first process to be run

src/modules/sensors/sensors.cpp

@@ -54,11 +54,13 @@
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/time.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/differential_pressure.h>
@@ -82,7 +84,7 @@ using namespace time_literals;
  * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
  */
 #define PCB_TEMP_ESTIMATE_DEG		5.0f
-class Sensors : public ModuleBase<Sensors>, public ModuleParams
+class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	explicit Sensors(bool hil_enabled);
@@ -91,9 +93,6 @@ public:
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static Sensors *instantiate(int argc, char *argv[]);
-
 	/** @see ModuleBase */
 	static int custom_command(int argc, char *argv[]);
 
@@ -101,15 +100,25 @@ public:
 	static int print_usage(const char *reason = nullptr);
 
 	/** @see ModuleBase::run() */
-	void run() override;
+	void Run() override;
 
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
+	bool init();
+
 private:
 	const bool	_hil_enabled;			/**< if true, HIL is active */
 	bool		_armed{false};				/**< arming status of the vehicle */
 
+	hrt_abstime     _last_config_update{0};
+	hrt_abstime     _airdata_prev_timestamp{0};
+	hrt_abstime     _sensor_combined_prev_timestamp{0};
+	hrt_abstime     _magnetometer_prev_timestamp{0};
+
+	sensor_combined_s _sensor_combined{};
+	sensor_preflight_s _sensor_preflight{};
+
 	uORB::Subscription	_actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};		/**< attitude controls sub */
 	uORB::Subscription	_diff_pres_sub{ORB_ID(differential_pressure)};			/**< raw differential pressure subscription */
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
@@ -117,10 +126,19 @@ private:
 
 	uORB::Publication<airspeed_s>			_airspeed_pub{ORB_ID(airspeed)};			/**< airspeed */
 	uORB::Publication<sensor_combined_s>		_sensor_pub{ORB_ID(sensor_combined)};			/**< combined sensor data topic */
-	uORB::Publication<sensor_preflight_s>		_sensor_preflight{ORB_ID(sensor_preflight)};		/**< sensor preflight topic */
+	uORB::Publication<sensor_preflight_s>		_sensor_preflight_pub{ORB_ID(sensor_preflight)};		/**< sensor preflight topic */
 	uORB::Publication<vehicle_air_data_s>		_airdata_pub{ORB_ID(vehicle_air_data)};			/**< combined sensor data topic */
 	uORB::Publication<vehicle_magnetometer_s>	_magnetometer_pub{ORB_ID(vehicle_magnetometer)};	/**< combined sensor data topic */
 
+	uORB::SubscriptionCallbackWorkItem _sensor_gyro_integrated_sub[GYRO_COUNT_MAX] {
+		{this, ORB_ID(sensor_gyro_integrated), 0},
+		{this, ORB_ID(sensor_gyro_integrated), 1},
+		{this, ORB_ID(sensor_gyro_integrated), 2}
+	};
+
+	uint32_t _selected_sensor_device_id{0};
+	uint8_t _selected_sensor_sub_index{0};
+
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	DataValidator	_airspeed_validator;		/**< data validator to monitor airspeed */
@@ -183,6 +201,7 @@ private:
 
 Sensors::Sensors(bool hil_enabled) :
 	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl),
 	_hil_enabled(hil_enabled),
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
 	_voted_sensors_update(_parameters, hil_enabled)
@@ -210,8 +229,15 @@ Sensors::~Sensors()
 	}
 }
 
-int
-Sensors::parameters_update()
+bool Sensors::init()
+{
+	// initially run manually
+	ScheduleDelayed(10_ms);
+
+	return true;
+}
+
+int Sensors::parameters_update()
 {
 	if (_armed) {
 		return 0;
@@ -225,8 +251,7 @@ Sensors::parameters_update()
 	return PX4_OK;
 }
 
-int
-Sensors::adc_init()
+int Sensors::adc_init()
 {
 	if (!_hil_enabled) {
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
@@ -253,7 +278,7 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
 		float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature :
 						(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
-		airspeed_s airspeed;
+		airspeed_s airspeed{};
 		airspeed.timestamp = diff_pres.timestamp;
 
 		/* push data into validator */
@@ -333,8 +358,7 @@ Sensors::parameter_update_poll(bool forced)
 	}
 }
 
-void
-Sensors::adc_poll()
+void Sensors::adc_poll()
 {
 #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
 
@@ -421,143 +445,173 @@ void Sensors::InitializeVehicleIMU()
 	}
 }
 
-void
-Sensors::run()
+void Sensors::Run()
 {
-	adc_init();
+	if (should_exit()) {
+		// clear all registered callbacks
+		for (auto &sub : _sensor_gyro_integrated_sub) {
+			sub.unregisterCallback();
+		}
 
-	sensor_combined_s raw = {};
-	sensor_preflight_s preflt = {};
-	vehicle_air_data_s airdata = {};
-	vehicle_magnetometer_s magnetometer = {};
+		_voted_sensors_update.deinit();
+		exit_and_cleanup();
+		return;
+	}
 
-	_voted_sensors_update.init(raw);
+	// run once
+	if (_last_config_update == 0) {
+		adc_init();
+		_voted_sensors_update.init(_sensor_combined);
+		parameter_update_poll(true);
+	}
 
-	/* (re)load params and calibration */
-	parameter_update_poll(true);
+	perf_begin(_loop_perf);
 
-	/* get a set of initial values */
-	_voted_sensors_update.sensorsPoll(raw, airdata, magnetometer);
+	// backup schedule as a watchdog timeout
+	ScheduleDelayed(10_ms);
 
-	diff_pres_poll(airdata);
+	sensor_gyro_integrated_s gyro_integrated;
+	_sensor_gyro_integrated_sub[_selected_sensor_sub_index].copy(&gyro_integrated);
 
-	/* wakeup source */
-	px4_pollfd_struct_t poll_fds = {};
-	poll_fds.events = POLLIN;
+	// check vehicle status for changes to publication state
+	if (_vcontrol_mode_sub.updated()) {
+		vehicle_control_mode_s vcontrol_mode{};
 
-	uint64_t last_config_update = hrt_absolute_time();
+		if (_vcontrol_mode_sub.copy(&vcontrol_mode)) {
+			_armed = vcontrol_mode.flag_armed;
+		}
+	}
 
-	while (!should_exit()) {
+	vehicle_air_data_s airdata{};
+	vehicle_magnetometer_s magnetometer{};
+	_voted_sensors_update.sensorsPoll(_sensor_combined, airdata, magnetometer);
 
-		/* use the best-voted gyro to pace output */
-		poll_fds.fd = _voted_sensors_update.bestGyroFd();
+	// check analog airspeed
+	adc_poll();
 
-		/* wait for up to 50ms for data (Note that this implies, we can have a fail-over time of 50ms,
-		 * if a gyro fails) */
-		int pret = px4_poll(&poll_fds, 1, 50);
+	diff_pres_poll(airdata);
 
-		/* If pret == 0 it timed out but we should still do all checks and potentially copy
-		 * other gyros. */
 
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
-			/* if the polling operation failed because no gyro sensor is available yet,
-			 * then attempt to subscribe once again
-			 */
-			if (_voted_sensors_update.numGyros() == 0) {
-				_voted_sensors_update.initializeSensors();
-			}
+	// check failover and update subscribed sensor (if necessary)
+	_voted_sensors_update.checkFailover();
+	const uint32_t current_device_id = _voted_sensors_update.bestGyroID();
 
-			px4_usleep(1000);
-			continue;
+	if (_selected_sensor_device_id != current_device_id) {
+		// clear all registered callbacks
+		for (auto &sub : _sensor_gyro_integrated_sub) {
+			sub.unregisterCallback();
 		}
 
-		perf_begin(_loop_perf);
+		for (int i = 0; i < GYRO_COUNT_MAX; i++) {
+			sensor_gyro_integrated_s report{};
 
-		/* check vehicle status for changes to publication state */
-		if (_vcontrol_mode_sub.updated()) {
-			vehicle_control_mode_s vcontrol_mode{};
-			_vcontrol_mode_sub.copy(&vcontrol_mode);
-			_armed = vcontrol_mode.flag_armed;
+			if (_sensor_gyro_integrated_sub[i].copy(&report)) {
+				if ((report.device_id != 0) && (report.device_id == current_device_id)) {
+					if (_sensor_gyro_integrated_sub[i].registerCallback()) {
+						// record selected sensor (array index)
+						_selected_sensor_sub_index = i;
+						_selected_sensor_device_id = current_device_id;
+					}
+				}
+			}
 		}
+	}
 
-		/* the timestamp of the raw struct is updated by the gyroPoll() method (this makes the gyro
-		 * a mandatory sensor) */
-		const uint64_t airdata_prev_timestamp = airdata.timestamp;
-		const uint64_t magnetometer_prev_timestamp = magnetometer.timestamp;
+	if ((airdata.timestamp != 0) && (airdata.timestamp != _airdata_prev_timestamp)) {
+		_airdata_pub.publish(airdata);
+		_airdata_prev_timestamp = airdata.timestamp;
+	}
 
-		_voted_sensors_update.sensorsPoll(raw, airdata, magnetometer);
+	if ((magnetometer.timestamp != 0) && (magnetometer.timestamp != _magnetometer_prev_timestamp)) {
+		_magnetometer_pub.publish(magnetometer);
+		_magnetometer_prev_timestamp = magnetometer.timestamp;
 
-		/* check analog airspeed */
-		adc_poll();
+		if (!_armed) {
+			_voted_sensors_update.calcMagInconsistency(_sensor_preflight);
+		}
+	}
 
-		diff_pres_poll(airdata);
+	if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
 
-		if (raw.timestamp > 0) {
+		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
+		_sensor_pub.publish(_sensor_combined);
+		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
 
-			_voted_sensors_update.setRelativeTimestamps(raw);
+		// If the the vehicle is disarmed calculate the length of the maximum difference between
+		// IMU units as a consistency metric and publish to the sensor preflight topic
+		if (!_armed) {
+			_voted_sensors_update.calcAccelInconsistency(_sensor_preflight);
+			_voted_sensors_update.calcGyroInconsistency(_sensor_preflight);
 
-			_sensor_pub.publish(raw);
+			_sensor_preflight.timestamp = hrt_absolute_time();
+			_sensor_preflight_pub.publish(_sensor_preflight);
+		}
+	}
 
-			if (airdata.timestamp != airdata_prev_timestamp) {
-				_airdata_pub.publish(airdata);
-			}
+	// 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) > 500_ms) {
+		_voted_sensors_update.initializeSensors();
+		InitializeVehicleIMU();
+		_last_config_update = hrt_absolute_time();
 
-			if (magnetometer.timestamp != magnetometer_prev_timestamp) {
-				_magnetometer_pub.publish(magnetometer);
-			}
+	} else {
+		// check parameters for updates
+		parameter_update_poll();
+	}
 
-			_voted_sensors_update.checkFailover();
+	perf_end(_loop_perf);
+}
 
-			/* If the the vehicle is disarmed calculate the length of the maximum difference between
-			 * IMU units as a consistency metric and publish to the sensor preflight topic
-			*/
-			if (!_armed) {
-				preflt.timestamp = hrt_absolute_time();
-				_voted_sensors_update.calcAccelInconsistency(preflt);
-				_voted_sensors_update.calcGyroInconsistency(preflt);
-				_voted_sensors_update.calcMagInconsistency(preflt);
+int Sensors::task_spawn(int argc, char *argv[])
+{
+	bool hil_enabled = false;
+	bool error_flag = false;
 
-				_sensor_preflight.publish(preflt);
-			}
-		}
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
 
-		/* 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) > 500_ms) {
-			_voted_sensors_update.initializeSensors();
-			InitializeVehicleIMU();
-			last_config_update = hrt_absolute_time();
+	while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'h':
+			hil_enabled = true;
+			break;
 
-		} else {
+		case '?':
+			error_flag = true;
+			break;
 
-			/* check parameters for updates */
-			parameter_update_poll();
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
 		}
+	}
 
-		perf_end(_loop_perf);
+	if (error_flag) {
+		return PX4_ERROR;
 	}
 
-	_voted_sensors_update.deinit();
-}
+	Sensors *instance = new Sensors(hil_enabled);
 
-int Sensors::task_spawn(int argc, char *argv[])
-{
-	/* start the task */
-	_task_id = px4_task_spawn_cmd("sensors",
-				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_SENSOR_HUB,
-				      2000,
-				      (px4_main_t)&run_trampoline,
-				      (char *const *)argv);
-
-	if (_task_id < 0) {
-		_task_id = -1;
-		return -errno;
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
 	}
 
-	return 0;
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
 }
 
 int Sensors::print_status()
@@ -623,39 +677,6 @@ It runs in its own thread and polls on the currently selected gyro topic.
 	return 0;
 }
 
-Sensors *Sensors::instantiate(int argc, char *argv[])
-{
-	bool hil_enabled = false;
-	bool error_flag = false;
-
-	int myoptind = 1;
-	int ch;
-	const char *myoptarg = nullptr;
-
-	while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) {
-		switch (ch) {
-		case 'h':
-			hil_enabled = true;
-			break;
-
-		case '?':
-			error_flag = true;
-			break;
-
-		default:
-			PX4_WARN("unrecognized flag");
-			error_flag = true;
-			break;
-		}
-	}
-
-	if (error_flag) {
-		return nullptr;
-	}
-
-	return new Sensors(hil_enabled);
-}
-
 extern "C" __EXPORT int sensors_main(int argc, char *argv[])
 {
 	return Sensors::main(argc, argv);

src/modules/sensors/voted_sensors_update.h

@@ -125,11 +125,7 @@ public:
 	 */
 	void checkFailover();
 
-	int numGyros() const { return _gyro.subscription_count; }
-
-	int gyroFd(int idx) const { return _gyro.subscription[idx]; }
-
-	int bestGyroFd() const { return _gyro.subscription[_gyro.last_best_vote]; }
+	int bestGyroID() const { return _gyro_device_id[_gyro.last_best_vote]; }
 
 	/**
 	 * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor