sensor accel/gyro message cleanup

- split out integrated data into new standalone messages (sensor_accel_integrated and sensor_gyro_integrated)
 - publish sensor_gyro at full rate and remove sensor_gyro_control
 - limit sensor status publications to 10 Hz
 - remove unused accel/gyro raw ADC fields
 - add device IDs to sensor_bias and sensor_correction
    - vehicle_angular_velocity/vehicle_acceleration: check device ids before using bias and corrections

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -33,5 +33,6 @@
 
 px4_add_library(vehicle_acceleration
 	VehicleAcceleration.cpp
+	VehicleAcceleration.hpp
 )
 target_link_libraries(vehicle_acceleration PRIVATE px4_work_queue)

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -49,26 +49,18 @@ VehicleAcceleration::~VehicleAcceleration()
 	Stop();
 }
 
-bool
-VehicleAcceleration::Start()
+bool VehicleAcceleration::Start()
 {
-	// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
-	_scale = Vector3f{1.0f, 1.0f, 1.0f};
-	_offset.zero();
-	_bias.zero();
-
 	// force initial updates
 	ParametersUpdate(true);
 	SensorBiasUpdate(true);
+	SensorCorrectionsUpdate(true);
 
 	// needed to change the active sensor if the primary stops updating
-	_sensor_selection_sub.registerCallback();
-
-	return SensorCorrectionsUpdate(true);
+	return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
 }
 
-void
-VehicleAcceleration::Stop()
+void VehicleAcceleration::Stop()
 {
 	Deinit();
 
@@ -80,21 +72,23 @@ VehicleAcceleration::Stop()
 	_sensor_selection_sub.unregisterCallback();
 }
 
-void
-VehicleAcceleration::SensorBiasUpdate(bool force)
+void VehicleAcceleration::SensorBiasUpdate(bool force)
 {
 	if (_sensor_bias_sub.updated() || force) {
 		sensor_bias_s bias;
 
 		if (_sensor_bias_sub.copy(&bias)) {
-			// TODO: should be checking device ID
-			_bias = Vector3f{bias.accel_bias};
+			if (bias.accel_device_id == _selected_sensor_device_id) {
+				_bias = Vector3f{bias.accel_bias};
+
+			} else {
+				_bias.zero();
+			}
 		}
 	}
 }
 
-bool
-VehicleAcceleration::SensorCorrectionsUpdate(bool force)
+void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
 {
 	// check if the selected sensor has updated
 	if (_sensor_correction_sub.updated() || force) {
@@ -102,49 +96,82 @@ VehicleAcceleration::SensorCorrectionsUpdate(bool force)
 		sensor_correction_s corrections{};
 		_sensor_correction_sub.copy(&corrections);
 
-		// TODO: should be checking device ID
-		if (_selected_sensor == 0) {
+		// selected sensor has changed, find updated index
+		if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) {
+			_corrections_selected_instance = -1;
+
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.accel_device_ids[i] == _selected_sensor_device_id) {
+					_corrections_selected_instance = i;
+				}
+			}
+		}
+
+		switch (_corrections_selected_instance) {
+		case 0:
 			_offset = Vector3f{corrections.accel_offset_0};
 			_scale = Vector3f{corrections.accel_scale_0};
-
-		} else if (_selected_sensor == 1) {
+			break;
+		case 1:
 			_offset = Vector3f{corrections.accel_offset_1};
 			_scale = Vector3f{corrections.accel_scale_1};
-
-		} else if (_selected_sensor == 2) {
+			break;
+		case 2:
 			_offset = Vector3f{corrections.accel_offset_2};
 			_scale = Vector3f{corrections.accel_scale_2};
-
-		} else {
+			break;
+		default:
 			_offset = Vector3f{0.0f, 0.0f, 0.0f};
 			_scale = Vector3f{1.0f, 1.0f, 1.0f};
 		}
+	}
+}
 
-		// update the latest sensor selection
-		if ((_selected_sensor != corrections.selected_accel_instance) || force) {
-			if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) {
-				// clear all registered callbacks
-				for (auto &sub : _sensor_sub) {
-					sub.unregisterCallback();
-				}
+bool VehicleAcceleration::SensorSelectionUpdate(bool force)
+{
+	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
+		sensor_selection_s sensor_selection{};
+		_sensor_selection_sub.copy(&sensor_selection);
 
-				const int sensor_new = corrections.selected_accel_instance;
+		if (_selected_sensor_device_id != sensor_selection.accel_device_id) {
+			// clear all registered callbacks
+			for (auto &sub : _sensor_sub) {
+				sub.unregisterCallback();
+			}
 
-				if (_sensor_sub[sensor_new].registerCallback()) {
-					PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
-					_selected_sensor = sensor_new;
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				sensor_accel_s report{};
+				_sensor_sub[i].copy(&report);
 
-					return true;
+				if ((report.device_id != 0) && (report.device_id == sensor_selection.accel_device_id)) {
+					if (_sensor_sub[i].registerCallback()) {
+						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
+
+						// record selected sensor (array index)
+						_selected_sensor_sub_index = i;
+						_selected_sensor_device_id = sensor_selection.accel_device_id;
+
+						// clear bias and corrections
+						_bias.zero();
+						_offset = Vector3f{0.0f, 0.0f, 0.0f};
+						_scale = Vector3f{1.0f, 1.0f, 1.0f};
+
+						return true;
+					}
 				}
 			}
+
+			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id);
+			_selected_sensor_device_id = 0;
+			_selected_sensor_sub_index = 0;
 		}
 	}
 
 	return false;
 }
 
-void
-VehicleAcceleration::ParametersUpdate(bool force)
+void VehicleAcceleration::ParametersUpdate(bool force)
 {
 	// Check if parameters have changed
 	if (_params_sub.updated() || force) {
@@ -167,20 +194,19 @@ VehicleAcceleration::ParametersUpdate(bool force)
 	}
 }
 
-void
-VehicleAcceleration::Run()
+void VehicleAcceleration::Run()
 {
 	// update corrections first to set _selected_sensor
-	bool sensor_select_update = SensorCorrectionsUpdate();
+	bool sensor_select_update = SensorSelectionUpdate();
+	SensorCorrectionsUpdate(sensor_select_update);
+	SensorBiasUpdate(sensor_select_update);
+	ParametersUpdate();
 
-	if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
+	if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
 		sensor_accel_s sensor_data;
-		_sensor_sub[_selected_sensor].copy(&sensor_data);
+		_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
 
-		ParametersUpdate();
-		SensorBiasUpdate();
-
-		// get the sensor data and correct for thermal errors
+		// get the sensor data and correct for thermal errors (apply offsets and scale)
 		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
 		// apply offsets and scale
@@ -192,8 +218,10 @@ VehicleAcceleration::Run()
 		// correct for in-run bias errors
 		accel -= _bias;
 
-		vehicle_acceleration_s out{};
-		out.timestamp_sample = sensor_data.timestamp;
+		// publish
+		vehicle_acceleration_s out;
+
+		out.timestamp_sample = sensor_data.timestamp_sample;
 		accel.copyTo(out.xyz);
 		out.timestamp = hrt_absolute_time();
 
@@ -201,8 +229,10 @@ VehicleAcceleration::Run()
 	}
 }
 
-void
-VehicleAcceleration::PrintStatus()
+void VehicleAcceleration::PrintStatus()
 {
-	PX4_INFO("selected sensor: %d", _selected_sensor);
+	PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
+	PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
+	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
 }

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -56,7 +56,7 @@ class VehicleAcceleration : public ModuleParams, public px4::WorkItem
 public:
 
 	VehicleAcceleration();
-	virtual ~VehicleAcceleration();
+	~VehicleAcceleration() override;
 
 	void	Run() override;
 
@@ -66,10 +66,10 @@ public:
 	void	PrintStatus();
 
 private:
-
-	void	ParametersUpdate(bool force = false);
-	void	SensorBiasUpdate(bool force = false);
-	bool	SensorCorrectionsUpdate(bool force = false);
+	void ParametersUpdate(bool force = false);
+	void SensorBiasUpdate(bool force = false);
+	void SensorCorrectionsUpdate(bool force = false);
+	bool SensorSelectionUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
@@ -96,10 +96,11 @@ private:
 
 	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
 
-	matrix::Vector3f			_offset;
-	matrix::Vector3f			_scale;
-	matrix::Vector3f			_bias;
-
-	uint8_t					_selected_sensor{0};
+	matrix::Vector3f			_offset{0.f, 0.f, 0.f};
+	matrix::Vector3f			_scale{1.f, 1.f, 1.f};
+	matrix::Vector3f			_bias{0.f, 0.f, 0.f};
 
+	uint32_t				_selected_sensor_device_id{0};
+	uint8_t					_selected_sensor_sub_index{0};
+	int8_t					_corrections_selected_instance{-1};
 };

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -33,5 +33,6 @@
 
 px4_add_library(vehicle_angular_velocity
 	VehicleAngularVelocity.cpp
+	VehicleAngularVelocity.hpp
 )
 target_link_libraries(vehicle_angular_velocity PRIVATE px4_work_queue)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -49,26 +49,18 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
 	Stop();
 }
 
-bool
-VehicleAngularVelocity::Start()
+bool VehicleAngularVelocity::Start()
 {
-	// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
-	_scale = Vector3f{1.0f, 1.0f, 1.0f};
-	_offset.zero();
-	_bias.zero();
-
 	// force initial updates
 	ParametersUpdate(true);
 	SensorBiasUpdate(true);
+	SensorCorrectionsUpdate(true);
 
 	// needed to change the active sensor if the primary stops updating
-	_sensor_selection_sub.registerCallback();
-
-	return SensorCorrectionsUpdate(true);
+	return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
 }
 
-void
-VehicleAngularVelocity::Stop()
+void VehicleAngularVelocity::Stop()
 {
 	Deinit();
 
@@ -80,110 +72,106 @@ VehicleAngularVelocity::Stop()
 	_sensor_selection_sub.unregisterCallback();
 }
 
-void
-VehicleAngularVelocity::SensorBiasUpdate(bool force)
+void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 {
 	if (_sensor_bias_sub.updated() || force) {
 		sensor_bias_s bias;
 
 		if (_sensor_bias_sub.copy(&bias)) {
-			// TODO: should be checking device ID
-			_bias = Vector3f{bias.gyro_bias};
+			if (bias.gyro_device_id == _selected_sensor_device_id) {
+				_bias = Vector3f{bias.gyro_bias};
+
+			} else {
+				_bias.zero();
+			}
 		}
 	}
 }
 
-bool
-VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
+void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
 {
-	if (_sensor_selection_sub.updated() || force) {
-		sensor_selection_s sensor_selection;
-
-		if (_sensor_selection_sub.copy(&sensor_selection)) {
-			if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
-				_selected_sensor_device_id = sensor_selection.gyro_device_id;
-				force = true;
-			}
-		}
-	}
-
 	// check if the selected sensor has updated
 	if (_sensor_correction_sub.updated() || force) {
 
 		sensor_correction_s corrections{};
 		_sensor_correction_sub.copy(&corrections);
 
-		// TODO: should be checking device ID
-		if (_selected_sensor == 0) {
+		// selected sensor has changed, find updated index
+		if ((_corrections_selected_instance != corrections.selected_gyro_instance) || force) {
+			_corrections_selected_instance = -1;
+
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.gyro_device_ids[i] == _selected_sensor_device_id) {
+					_corrections_selected_instance = i;
+				}
+			}
+		}
+
+		switch (_corrections_selected_instance) {
+		case 0:
 			_offset = Vector3f{corrections.gyro_offset_0};
 			_scale = Vector3f{corrections.gyro_scale_0};
-
-		} else if (_selected_sensor == 1) {
+			break;
+		case 1:
 			_offset = Vector3f{corrections.gyro_offset_1};
 			_scale = Vector3f{corrections.gyro_scale_1};
-
-		} else if (_selected_sensor == 2) {
+			break;
+		case 2:
 			_offset = Vector3f{corrections.gyro_offset_2};
 			_scale = Vector3f{corrections.gyro_scale_2};
-
-		} else {
+			break;
+		default:
 			_offset = Vector3f{0.0f, 0.0f, 0.0f};
 			_scale = Vector3f{1.0f, 1.0f, 1.0f};
 		}
+	}
+}
 
-		// update the latest sensor selection
-		if ((_selected_sensor != corrections.selected_gyro_instance) || force) {
-			if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) {
-				// clear all registered callbacks
-				for (auto &sub : _sensor_control_sub) {
-					sub.unregisterCallback();
-				}
+bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
+{
+	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
+		sensor_selection_s sensor_selection{};
+		_sensor_selection_sub.copy(&sensor_selection);
 
-				for (auto &sub : _sensor_sub) {
-					sub.unregisterCallback();
-				}
+		if (_selected_sensor_device_id != sensor_selection.gyro_device_id) {
+			// clear all registered callbacks
+			for (auto &sub : _sensor_sub) {
+				sub.unregisterCallback();
+			}
 
-				const int sensor_new = corrections.selected_gyro_instance;
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				sensor_gyro_s report{};
+				_sensor_sub[i].copy(&report);
 
-				// subscribe to sensor_gyro_control if available
-				//  currently not all drivers (eg df_*) provide sensor_gyro_control
-				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-					sensor_gyro_control_s report{};
-					_sensor_control_sub[i].copy(&report);
+				if ((report.device_id != 0) && (report.device_id == sensor_selection.gyro_device_id)) {
+					if (_sensor_sub[i].registerCallback()) {
+						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);
 
-					if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) {
-						if (_sensor_control_sub[i].registerCallback()) {
-							PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i);
-							_selected_sensor_control = i;
+						// record selected sensor (array index)
+						_selected_sensor_sub_index = i;
+						_selected_sensor_device_id = sensor_selection.gyro_device_id;
 
-							_sensor_control_available = true;
+						// clear bias and corrections
+						_bias.zero();
+						_offset = Vector3f{0.0f, 0.0f, 0.0f};
+						_scale = Vector3f{1.0f, 1.0f, 1.0f};
 
-							// record selected sensor (sensor_gyro orb index)
-							_selected_sensor = sensor_new;
-
-							return true;
-						}
+						return true;
 					}
 				}
-
-				// otherwise fallback to using sensor_gyro (legacy that will be removed)
-				_sensor_control_available = false;
-
-				if (_sensor_sub[sensor_new].registerCallback()) {
-					PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new);
-					_selected_sensor = sensor_new;
-
-					return true;
-				}
 			}
+
+			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.gyro_device_id);
+			_selected_sensor_device_id = 0;
+			_selected_sensor_sub_index = 0;
 		}
 	}
 
 	return false;
 }
 
-void
-VehicleAngularVelocity::ParametersUpdate(bool force)
+void VehicleAngularVelocity::ParametersUpdate(bool force)
 {
 	// Check if parameters have changed
 	if (_params_sub.updated() || force) {
@@ -206,78 +194,45 @@ VehicleAngularVelocity::ParametersUpdate(bool force)
 	}
 }
 
-void
-VehicleAngularVelocity::Run()
+void VehicleAngularVelocity::Run()
 {
 	// update corrections first to set _selected_sensor
-	bool sensor_select_update = SensorCorrectionsUpdate();
+	bool sensor_select_update = SensorSelectionUpdate();
+	SensorCorrectionsUpdate(sensor_select_update);
+	SensorBiasUpdate(sensor_select_update);
+	ParametersUpdate();
 
-	if (_sensor_control_available) {
-		//  using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control
-		if (_sensor_control_sub[_selected_sensor].updated() || sensor_select_update) {
-			sensor_gyro_control_s sensor_data;
-			_sensor_control_sub[_selected_sensor].copy(&sensor_data);
+	if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
+		sensor_gyro_s sensor_data;
+		_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
 
-			ParametersUpdate();
-			SensorBiasUpdate();
+		// get the sensor data and correct for thermal errors (apply offsets and scale)
+		const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
-			// get the sensor data and correct for thermal errors (apply offsets and scale)
-			Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)};
+		// apply offsets and scale
+		Vector3f rates{(val - _offset).emult(_scale)};
 
-			// rotate corrected measurements from sensor to body frame
-			rates = _board_rotation * rates;
+		// rotate corrected measurements from sensor to body frame
+		rates = _board_rotation * rates;
 
-			// correct for in-run bias errors
-			rates -= _bias;
+		// correct for in-run bias errors
+		rates -= _bias;
 
-			vehicle_angular_velocity_s angular_velocity;
-			angular_velocity.timestamp_sample = sensor_data.timestamp_sample;
-			rates.copyTo(angular_velocity.xyz);
-			angular_velocity.timestamp = hrt_absolute_time();
+		// publish
+		vehicle_angular_velocity_s out;
 
-			_vehicle_angular_velocity_pub.publish(angular_velocity);
-		}
+		out.timestamp_sample = sensor_data.timestamp_sample;
+		rates.copyTo(out.xyz);
+		out.timestamp = hrt_absolute_time();
 
-	} else {
-		// otherwise fallback to using sensor_gyro (legacy that will be removed)
-		if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) {
-			sensor_gyro_s sensor_data;
-			_sensor_sub[_selected_sensor].copy(&sensor_data);
-
-			ParametersUpdate();
-			SensorBiasUpdate();
-
-			// get the sensor data and correct for thermal errors
-			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
-
-			// apply offsets and scale
-			Vector3f rates{(val - _offset).emult(_scale)};
-
-			// rotate corrected measurements from sensor to body frame
-			rates = _board_rotation * rates;
-
-			// correct for in-run bias errors
-			rates -= _bias;
-
-			vehicle_angular_velocity_s angular_velocity;
-			angular_velocity.timestamp_sample = sensor_data.timestamp;
-			rates.copyTo(angular_velocity.xyz);
-			angular_velocity.timestamp = hrt_absolute_time();
-
-			_vehicle_angular_velocity_pub.publish(angular_velocity);
-		}
+		_vehicle_angular_velocity_pub.publish(out);
 	}
 }
 
-void
-VehicleAngularVelocity::PrintStatus()
+void VehicleAngularVelocity::PrintStatus()
 {
-	PX4_INFO("selected sensor: %d", _selected_sensor);
-
-	if (_selected_sensor_device_id != 0) {
-		PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control);
-
-	} else {
-		PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id,  _selected_sensor);
-	}
+	PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
+	PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	PX4_INFO("offset: [%.3f %.3f %.3f]", (double)_offset(0), (double)_offset(1), (double)_offset(2));
+	PX4_INFO("scale: [%.3f %.3f %.3f]", (double)_scale(0), (double)_scale(1), (double)_scale(2));
 }

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -49,7 +49,6 @@
 #include <uORB/topics/sensor_selection.h>
 
 #include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_gyro_control.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 
 class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
@@ -57,7 +56,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
 public:
 
 	VehicleAngularVelocity();
-	virtual ~VehicleAngularVelocity();
+	~VehicleAngularVelocity() override;
 
 	void	Run() override;
 
@@ -67,10 +66,10 @@ public:
 	void	PrintStatus();
 
 private:
-
-	void	ParametersUpdate(bool force = false);
-	void	SensorBiasUpdate(bool force = false);
-	bool	SensorCorrectionsUpdate(bool force = false);
+	void ParametersUpdate(bool force = false);
+	void SensorBiasUpdate(bool force = false);
+	void SensorCorrectionsUpdate(bool force = false);
+	bool SensorSelectionUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
@@ -89,28 +88,19 @@ private:
 	uORB::Subscription			_sensor_correction_sub{ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
 
 	uORB::SubscriptionCallbackWorkItem	_sensor_selection_sub{this, ORB_ID(sensor_selection)};	/**< selected primary sensor subscription */
-
 	uORB::SubscriptionCallbackWorkItem	_sensor_sub[MAX_SENSOR_COUNT] {				/**< sensor data subscription */
 		{this, ORB_ID(sensor_gyro), 0},
 		{this, ORB_ID(sensor_gyro), 1},
 		{this, ORB_ID(sensor_gyro), 2}
 	};
 
-	uORB::SubscriptionCallbackWorkItem	_sensor_control_sub[MAX_SENSOR_COUNT] {			/**< sensor control data subscription */
-		{this, ORB_ID(sensor_gyro_control), 0},
-		{this, ORB_ID(sensor_gyro_control), 1},
-		{this, ORB_ID(sensor_gyro_control), 2}
-	};
-
 	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
 
-	matrix::Vector3f			_offset;
-	matrix::Vector3f			_scale;
-	matrix::Vector3f			_bias;
+	matrix::Vector3f			_bias{0.f, 0.f, 0.f};
+	matrix::Vector3f			_offset{0.f, 0.f, 0.f};
+	matrix::Vector3f			_scale{1.f, 1.f, 1.f};
 
 	uint32_t				_selected_sensor_device_id{0};
-	uint8_t					_selected_sensor{0};
-	uint8_t					_selected_sensor_control{0};
-	bool					_sensor_control_available{false};
-
+	uint8_t					_selected_sensor_sub_index{0};
+	int8_t					_corrections_selected_instance{-1};
 };

src/modules/sensors/voted_sensors_update.cpp

@@ -92,9 +92,9 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
 
 void VotedSensorsUpdate::initializeSensors()
 {
-	initSensorClass(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX);
+	initSensorClass(ORB_ID(sensor_gyro_integrated), _gyro, GYRO_COUNT_MAX);
 	initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX);
-	initSensorClass(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX);
+	initSensorClass(ORB_ID(sensor_accel_integrated), _accel, ACCEL_COUNT_MAX);
 	initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX);
 }
 
@@ -142,7 +142,7 @@ void VotedSensorsUpdate::parametersUpdate()
 	/* gyro */
 	for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
 
-		uORB::SubscriptionData<sensor_gyro_s> report{ORB_ID(sensor_gyro), topic_instance};
+		uORB::SubscriptionData<sensor_gyro_integrated_s> report{ORB_ID(sensor_gyro_integrated), topic_instance};
 
 		int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
 
@@ -151,10 +151,11 @@ void VotedSensorsUpdate::parametersUpdate()
 				topic_instance);
 
 			_corrections.gyro_mapping[topic_instance] = 0;
+			_corrections.gyro_device_ids[topic_instance] = 0;
 
 		} else {
 			_corrections.gyro_mapping[topic_instance] = temp;
-
+			_corrections.gyro_device_ids[topic_instance] = report.get().device_id;
 		}
 	}
 
@@ -162,7 +163,7 @@ void VotedSensorsUpdate::parametersUpdate()
 	/* accel */
 	for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
 
-		uORB::SubscriptionData<sensor_accel_s> report{ORB_ID(sensor_accel), topic_instance};
+		uORB::SubscriptionData<sensor_accel_integrated_s> report{ORB_ID(sensor_accel_integrated), topic_instance};
 
 		int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
 
@@ -171,10 +172,11 @@ void VotedSensorsUpdate::parametersUpdate()
 				topic_instance);
 
 			_corrections.accel_mapping[topic_instance] = 0;
+			_corrections.accel_device_ids[topic_instance] = 0;
 
 		} else {
 			_corrections.accel_mapping[topic_instance] = temp;
-
+			_corrections.accel_device_ids[topic_instance] = report.get().device_id;
 		}
 	}
 
@@ -191,10 +193,11 @@ void VotedSensorsUpdate::parametersUpdate()
 				topic_instance);
 
 			_corrections.baro_mapping[topic_instance] = 0;
+			_corrections.baro_device_ids[topic_instance] = 0;
 
 		} else {
 			_corrections.baro_mapping[topic_instance] = temp;
-
+			_corrections.baro_device_ids[topic_instance] = report.get().device_id;
 		}
 	}
 
@@ -565,9 +568,9 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
 		orb_check(_accel.subscription[uorb_index], &accel_updated);
 
 		if (accel_updated) {
-			sensor_accel_s accel_report;
+			sensor_accel_integrated_s accel_report;
 
-			int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report);
+			int ret = orb_copy(ORB_ID(sensor_accel_integrated), _accel.subscription[uorb_index], &accel_report);
 
 			if (ret != PX4_OK || accel_report.timestamp == 0) {
 				continue; //ignore invalid data
@@ -586,40 +589,17 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
 
 			_accel_device_id[uorb_index] = accel_report.device_id;
 
-			Vector3f accel_data;
+			/*
+			 * Correct the raw sensor data for scale factor errors
+			 * and offsets due to temperature variation. It is assumed that any filtering of input
+			 * data required is performed in the sensor driver, preferably before downsampling.
+			*/
 
-			if (accel_report.integral_dt != 0) {
-				/*
-				 * Using data that has been integrated in the driver before downsampling is preferred
-				 * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
-				 * and offsets due to temperature variation. It is assumed that any filtering of input
-				 * data required is performed in the sensor driver, preferably before downsampling.
-				*/
+			// convert the delta velocities to an equivalent acceleration before application of corrections
+			const float dt_inv = 1.e6f / (float)accel_report.dt;
+			Vector3f accel_data = Vector3f{accel_report.delta_velocity} * dt_inv;
 
-				// convert the delta velocities to an equivalent acceleration before application of corrections
-				float dt_inv = 1.e6f / accel_report.integral_dt;
-				accel_data = Vector3f(accel_report.x_integral * dt_inv,
-						      accel_report.y_integral * dt_inv,
-						      accel_report.z_integral * dt_inv);
-
-				_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt;
-
-			} else {
-				// using the value instead of the integral (the integral is the prefered choice)
-
-				// Correct each sensor for temperature effects
-				// Filtering and/or downsampling of temperature should be performed in the driver layer
-				accel_data = Vector3f(accel_report.x, accel_report.y, accel_report.z);
-
-				// handle the cse where this is our first output
-				if (_last_accel_timestamp[uorb_index] == 0) {
-					_last_accel_timestamp[uorb_index] = accel_report.timestamp - 1000;
-				}
-
-				// approximate the  delta time using the difference in accel data time stamps
-				_last_sensor_data[uorb_index].accelerometer_integral_dt =
-					(accel_report.timestamp - _last_accel_timestamp[uorb_index]);
-			}
+			_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
 
 			// handle temperature compensation
 			if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
@@ -672,9 +652,9 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
 		orb_check(_gyro.subscription[uorb_index], &gyro_updated);
 
 		if (gyro_updated) {
-			sensor_gyro_s gyro_report;
+			sensor_gyro_integrated_s gyro_report;
 
-			int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report);
+			int ret = orb_copy(ORB_ID(sensor_gyro_integrated), _gyro.subscription[uorb_index], &gyro_report);
 
 			if (ret != PX4_OK || gyro_report.timestamp == 0) {
 				continue; //ignore invalid data
@@ -693,40 +673,17 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
 
 			_gyro_device_id[uorb_index] = gyro_report.device_id;
 
-			Vector3f gyro_rate;
+			/*
+			 * Correct the raw sensor data for scale factor errors
+			 * and offsets due to temperature variation. It is assumed that any filtering of input
+			 * data required is performed in the sensor driver, preferably before downsampling.
+			*/
 
-			if (gyro_report.integral_dt != 0) {
-				/*
-				 * Using data that has been integrated in the driver before downsampling is preferred
-				 * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors
-				 * and offsets due to temperature variation. It is assumed that any filtering of input
-				 * data required is performed in the sensor driver, preferably before downsampling.
-				*/
+			// convert the delta angles to an equivalent angular rate before application of corrections
+			const float dt_inv = 1.e6f / (float)gyro_report.dt;
+			Vector3f gyro_rate = Vector3f{gyro_report.delta_angle} * dt_inv;
 
-				// convert the delta angles to an equivalent angular rate before application of corrections
-				float dt_inv = 1.e6f / gyro_report.integral_dt;
-				gyro_rate = Vector3f(gyro_report.x_integral * dt_inv,
-						     gyro_report.y_integral * dt_inv,
-						     gyro_report.z_integral * dt_inv);
-
-				_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt;
-
-			} else {
-				//using the value instead of the integral (the integral is the prefered choice)
-
-				// Correct each sensor for temperature effects
-				// Filtering and/or downsampling of temperature should be performed in the driver layer
-				gyro_rate = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
-
-				// handle the case where this is our first output
-				if (_last_sensor_data[uorb_index].timestamp == 0) {
-					_last_sensor_data[uorb_index].timestamp = gyro_report.timestamp - 1000;
-				}
-
-				// approximate the delta time using the difference in gyro data time stamps
-				_last_sensor_data[uorb_index].gyro_integral_dt =
-					(gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp);
-			}
+			_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
 
 			// handle temperature compensation
 			if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,

src/modules/sensors/voted_sensors_update.h

@@ -54,9 +54,11 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/sensor_accel_integrated.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_preflight.h>
 #include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/sensor_gyro_integrated.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_magnetometer.h>