voted_sensors_update: publish sensor selections (+3 squashed commits)

Squashed commits:
[290660d] voted_sensors_update: revert the new_accel_data check (and others)

The check removed the ability to detect sensor timeouts.
[c8dc7ad] sensors: publish changes to sensor selection
[dd90dec] sensors: ensure all sensor selections published first time

src/modules/sensors/voted_sensors_update.cpp

@@ -99,6 +99,7 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
 	initialize_sensors();
 
 	_corrections_changed = true; //make sure to initially publish the corrections topic
+	_selection_changed = true;
 
 	return 0;
 }
@@ -556,6 +557,8 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 				_accel.priority[uorb_index] = (uint8_t)priority;
 			}
 
+			_accel_device_id[uorb_index] = accel_report.device_id;
+
 			math::Vector<3> accel_data;
 
 			if (accel_report.integral_dt != 0) {
@@ -625,6 +628,11 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
 			_corrections_changed = true;
 		}
 
+		if (_selection.accel_device_id != _accel_device_id[best_index]) {
+			_selection_changed = true;
+			_selection.accel_device_id = _accel_device_id[best_index];
+		}
+
 		for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
 			raw.accelerometer_m_s2[axis_index] = _last_sensor_data[best_index].accelerometer_m_s2[axis_index];
 		}
@@ -656,6 +664,8 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 				_gyro.priority[uorb_index] = (uint8_t)priority;
 			}
 
+			_gyro_device_id[uorb_index] = gyro_report.device_id;
+
 			math::Vector<3> gyro_rate;
 
 			if (gyro_report.integral_dt != 0) {
@@ -726,6 +736,11 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
 			_corrections_changed = true;
 		}
 
+		if (_selection.gyro_device_id != _gyro_device_id[best_index]) {
+			_selection_changed = true;
+			_selection.gyro_device_id = _gyro_device_id[best_index];
+		}
+
 		for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
 			raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index];
 		}
@@ -754,6 +769,8 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 				_mag.priority[i] = (uint8_t)priority;
 			}
 
+			_mag_device_id[i] = mag_report.device_id;
+
 			math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 			vect = _mag_rotation[i] * vect;
 
@@ -776,6 +793,11 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 		raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
 		_mag.last_best_vote = (uint8_t)best_index;
 	}
+
+	if (_selection.mag_device_id != _mag_device_id[best_index]) {
+		_selection_changed = true;
+		_selection.mag_device_id = _mag_device_id[best_index];
+	}
 }
 
 void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
@@ -815,6 +837,8 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 				_baro.priority[uorb_index] = (uint8_t)priority;
 			}
 
+			_baro_device_id[uorb_index] = baro_report.device_id;
+
 			got_update = true;
 			math::Vector<3> vect(baro_report.altitude, 0.f, 0.f);
 
@@ -842,6 +866,11 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
 				_corrections_changed = true;
 			}
 
+			if (_selection.baro_device_id != _baro_device_id[best_index]) {
+				_selection_changed = true;
+				_selection.baro_device_id = _baro_device_id[best_index];
+			}
+
 			/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
 
 			/*
@@ -1041,11 +1070,24 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw)
 
 		} else {
 			orb_publish(ORB_ID(sensor_correction), _sensor_correction_pub, &_corrections);
-
 		}
 
 		_corrections_changed = false;
 	}
+
+	// publish sensor selection if changed
+	if (_selection_changed) {
+		_selection.timestamp = hrt_absolute_time();
+
+		if (_sensor_selection_pub == nullptr) {
+			_sensor_selection_pub = orb_advertise(ORB_ID(sensor_selection), &_selection);
+
+		} else {
+			orb_publish(ORB_ID(sensor_selection), _sensor_selection_pub, &_selection);
+		}
+
+		_selection_changed = false;
+	}
 }
 
 void VotedSensorsUpdate::check_failover()

src/modules/sensors/voted_sensors_update.h

@@ -55,6 +55,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_preflight.h>
 #include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/sensor_selection.h>
 
 #include <DevMgr.hpp>
 
@@ -269,6 +270,15 @@ private:
 	orb_advert_t _sensor_correction_pub = nullptr; /**< handle to the sensor correction uORB topic */
 	bool _corrections_changed = false;
 
+	/* sensor selection publication */
+	struct sensor_selection_s _selection = {}; /**< struct containing the sensor selection to be published to the uORB*/
+	orb_advert_t _sensor_selection_pub = nullptr; /**< handle to the sensor selection uORB topic */
+	bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */
+	uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {};
+	uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {};
+	uint32_t _gyro_device_id[SENSOR_COUNT_MAX] = {};
+	uint32_t _mag_device_id[SENSOR_COUNT_MAX] = {};
+
 	static const double	_msl_pressure;	/** average sea-level pressure in kPa */
 };