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};
 };