sensors temp compensation: do an orb_copy to get the driver ID

This removes the necessity, that the driver class ID matches the uorb topic
instance.

Also improve error handling & reporting

src/modules/sensors/temperature_compensation.cpp

@@ -315,16 +315,28 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(SensorCalData3D &coef, flo
 
 int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance)
 {
+	if (_parameters.gyro_tc_enable != 1) {
+		return 0;
+	}
+
 	return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data);
 }
 
 int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance)
 {
+	if (_parameters.accel_tc_enable != 1) {
+		return 0;
+	}
+
 	return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data);
 }
 
 int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance)
 {
+	if (_parameters.baro_tc_enable != 1) {
+		return 0;
+	}
+
 	return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data);
 }
 
@@ -346,10 +358,14 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc
 int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature,
 		float *offsets, float *scales)
 {
+	if (_parameters.gyro_tc_enable != 1) {
+		return 0;
+	}
+
 	uint8_t mapping = _gyro_data.device_mapping[topic_instance];
 
-	if (mapping == 255 || _parameters.gyro_tc_enable != 1) {
+	if (mapping == 255) {
-		return 0;
+		return -1;
 	}
 
 	calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets);
@@ -373,10 +389,14 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Ve
 int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data,
 		float temperature, float *offsets, float *scales)
 {
+	if (_parameters.accel_tc_enable != 1) {
+		return 0;
+	}
+
 	uint8_t mapping = _accel_data.device_mapping[topic_instance];
 
-	if (mapping == 255 || _parameters.accel_tc_enable != 1) {
+	if (mapping == 255) {
-		return 0;
+		return -1;
 	}
 
 	calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
@@ -400,10 +420,14 @@ int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::V
 int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
 		float *offsets, float *scales)
 {
+	if (_parameters.baro_tc_enable != 1) {
+		return 0;
+	}
+
 	uint8_t mapping = _baro_data.device_mapping[topic_instance];
 
-	if (mapping == 255 || _parameters.baro_tc_enable != 1) {
+	if (mapping == 255) {
-		return 0;
+		return -1;
 	}
 
 	calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets);

src/modules/sensors/temperature_compensation.h

@@ -78,7 +78,8 @@ public:
 	 * @param temperature measured current temperature
 	 * @param offsets returns offsets that were applied (length = 3, except for baro), depending on return value
 	 * @param scales returns scales that were applied (length = 3), depending on return value
-	 * @return 0: no changes (correction not enabled),
+	 * @return -1: error: correction enabled, but no sensor mapping set (@see set_sendor_id_gyro)
+	 *         0: no changes (correction not enabled),
 	 *         1: corrections applied but no changes to offsets & scales,
 	 *         2: corrections applied and offsets & scales updated
 	 */

src/modules/sensors/voted_sensors_update.cpp

@@ -140,8 +140,56 @@ void VotedSensorsUpdate::parameters_update()
 
 	_board_rotation = board_rotation_offset * _board_rotation;
 
+	/* Load & apply the sensor calibrations.
+	 * IMPORTANT: we assume all sensor drivers are running and published sensor data at this point
+	 */
+
+	/* temperature compensation */
 	_temperature_compensation.parameters_update();
 
+	for (unsigned topic_instance = 0; topic_instance < SENSOR_COUNT_MAX; ++topic_instance) {
+
+		/* gyro */
+		if (topic_instance < _gyro.subscription_count) {
+			// valid subscription, so get the driver id by getting the published sensor data
+			struct gyro_report report;
+
+			if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) {
+				if (_temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance) < 0) {
+					PX4_ERR("gyro temp compensation init: failed to find device ID %u for instance %i",
+						report.device_id, topic_instance);
+				}
+			}
+		}
+
+		/* accel */
+		if (topic_instance < _accel.subscription_count) {
+			// valid subscription, so get the driver id by getting the published sensor data
+			struct accel_report report;
+
+			if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) {
+				if (_temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance) < 0) {
+					PX4_ERR("accel temp compensation init: failed to find device ID %u for instance %i",
+						report.device_id, topic_instance);
+				}
+			}
+		}
+
+		/* baro */
+		if (topic_instance < _baro.subscription_count) {
+			// valid subscription, so get the driver id by getting the published sensor data
+			struct baro_report report;
+
+			if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) {
+				if (_temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance) < 0) {
+					PX4_ERR("baro temp compensation init: failed to find device ID %u for instance %i",
+						report.device_id, topic_instance);
+				}
+			}
+		}
+	}
+
+
 	/* set offset parameters to new values */
 	bool failed;
 	char str[30];
@@ -162,8 +210,6 @@ void VotedSensorsUpdate::parameters_update()
 		}
 
 		uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
-		_temperature_compensation.set_sensor_id_gyro(driver_device_id, s);
-
 		bool config_ok = false;
 
 		/* run through all stored calibrations that are applied at the driver level*/
@@ -230,8 +276,6 @@ void VotedSensorsUpdate::parameters_update()
 		}
 
 		uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
-		_temperature_compensation.set_sensor_id_accel(driver_device_id, s);
-
 		bool config_ok = false;
 
 		/* run through all stored calibrations */
@@ -305,7 +349,6 @@ void VotedSensorsUpdate::parameters_update()
 		}
 
 		uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
-
 		bool config_ok = false;
 
 		/* run through all stored calibrations */
@@ -421,21 +464,6 @@ void VotedSensorsUpdate::parameters_update()
 		}
 	}
 
-	/* run through all baro sensors */
-	for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
-
-		(void)sprintf(str, "%s%u", BARO_BASE_DEVICE_PATH, s);
-
-		DevHandle h;
-		DevMgr::getHandle(str, h);
-
-		if (!h.isValid()) {
-			continue;
-		}
-
-		uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
-		_temperature_compensation.set_sensor_id_baro(driver_device_id, s);
-	}
 }
 
 void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)

src/modules/sensors/voted_sensors_update.h

@@ -100,7 +100,8 @@ public:
 	float baro_pressure() const { return _last_best_baro_pressure; }
 
 	/**
-	 * call this whenever parameters got updated
+	 * call this whenever parameters got updated. Make sure to have initialize_sensors() called at least
+	 * once before calling this.
 	 */
 	void parameters_update();