sensor calibration: save temperature at calibration time for monitoring

src/modules/commander/accelerometer_calibration.cpp

@@ -157,13 +157,15 @@ struct accel_worker_data_s {
 	orb_advert_t	*mavlink_log_pub{nullptr};
 	unsigned	done_count{0};
 	float		accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
+	float		accel_temperature_ref[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
 };
 
 // Read specified number of accelerometer samples, calculate average and dispersion.
 static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
-		unsigned orient, unsigned samples_num)
+		float (&accel_temperature_avg)[MAX_ACCEL_SENS],	unsigned orient, unsigned samples_num)
 {
 	Vector3f accel_sum[MAX_ACCEL_SENS] {};
+	float temperature_sum[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
 	unsigned counts[MAX_ACCEL_SENS] {};
 
 	unsigned errcount = 0;
@@ -213,7 +215,16 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
 					}
 
 					accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
+
 					counts[accel_index]++;
+
+					if (!PX4_ISFINITE(temperature_sum[accel_index])) {
+						// set first valid value
+						temperature_sum[accel_index] = (arp.temperature * counts[accel_index]);
+
+					} else {
+						temperature_sum[accel_index] += arp.temperature;
+					}
 				}
 			}
 
@@ -237,6 +248,8 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
 	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
 		const Vector3f avg{accel_sum[s] / counts[s]};
 		avg.copyTo(accel_avg[s][orient]);
+
+		accel_temperature_avg[s] = temperature_sum[s] /  counts[s];
 	}
 
 	return calibrate_return_ok;
@@ -250,7 +263,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
 	calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
 			     detect_orientation_str(orientation));
 
-	read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
+	read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref, orientation, samples_num);
 
 	// check accel
 	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
@@ -404,6 +417,8 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 				const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
 				calibrations[i].set_scale(accel_T_rotated.diag());
 
+				calibrations[i].set_temperature(worker_data.accel_temperature_ref[i]);
+
 #if defined(DEBUD_BUILD)
 				PX4_INFO("accel %d: offset", i);
 				offset.print();
@@ -478,6 +493,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
 		sensor_accel_s arp{};
 		Vector3f accel_sum{};
+		float temperature_sum{NAN};
 		unsigned count = 0;
 
 		while (accel_subs[accel_index].update(&arp)) {
@@ -513,11 +529,21 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 
 					if (diff.norm() < 1.f) {
 						accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
+
 						count++;
+
+						if (!PX4_ISFINITE(temperature_sum)) {
+							// set first valid value
+							temperature_sum = (arp.temperature * count);
+
+						} else {
+							temperature_sum += arp.temperature;
+						}
 					}
 
 				} else {
 					accel_sum = accel;
+					temperature_sum = arp.temperature;
 					count = 1;
 				}
 			}
@@ -527,6 +553,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 
 			bool calibrated = false;
 			const Vector3f accel_avg = accel_sum / count;
+			const float temperature_avg = temperature_sum / count;
 
 			Vector3f offset{0.f, 0.f, 0.f};
 
@@ -572,6 +599,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 
 			} else {
 				calibration.set_offset(offset);
+				calibration.set_temperature(temperature_avg);
 
 				if (calibration.ParametersSave()) {
 					calibration.PrintStatus();

src/modules/commander/gyro_calibration.cpp

@@ -71,6 +71,7 @@ struct gyro_worker_data_t {
 	calibration::Gyroscope calibrations[MAX_GYROS] {};
 
 	Vector3f offset[MAX_GYROS] {};
+	float temperature[MAX_GYROS] {NAN, NAN, NAN, NAN};
 
 	math::MedianFilter<float, 9> filter[3] {};
 };
@@ -115,8 +116,17 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
 						const Vector3f &thermal_offset{worker_data.calibrations[gyro_index].thermal_offset()};
 
 						worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - thermal_offset;
+
 						calibration_counter[gyro_index]++;
 
+						if (!PX4_ISFINITE(worker_data.temperature[gyro_index])) {
+							// set first valid value
+							worker_data.temperature[gyro_index] = gyro_report.temperature * calibration_counter[gyro_index];
+
+						} else {
+							worker_data.temperature[gyro_index] += gyro_report.temperature;
+						}
+
 						if (gyro_index == 0) {
 							worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
 							worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
@@ -157,6 +167,7 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
 		}
 
 		worker_data.offset[s] /= calibration_counter[s];
+		worker_data.temperature[s] /= calibration_counter[s];
 	}
 
 	return calibrate_return_ok;
@@ -259,6 +270,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 			if (calibration.device_id() != 0) {
 				calibration.set_offset(worker_data.offset[uorb_index]);
+				calibration.set_temperature(worker_data.temperature[uorb_index]);
 
 				calibration.set_calibration_index(uorb_index);
 

src/modules/commander/mag_calibration.cpp

@@ -94,6 +94,8 @@ struct mag_worker_data_t {
 	float		*y[MAX_MAGS];
 	float		*z[MAX_MAGS];
 
+	float		temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
+
 	calibration::Magnetometer calibration[MAX_MAGS] {};
 };
 
@@ -340,6 +342,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		if (mag_sub[0].updatedBlocking(1000_ms)) {
 			bool rejected = false;
 			Vector3f new_samples[MAX_MAGS] {};
+			float new_temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
 
 			for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
 				if (worker_data->calibration[cur_mag].device_id() != 0) {
@@ -368,6 +371,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 
 						if (!reject) {
 							new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
+							new_temperature[cur_mag] = mag.temperature;
 							updated = true;
 							break;
 						}
@@ -387,6 +391,15 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 						worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](0);
 						worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1);
 						worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2);
+
+						if (!PX4_ISFINITE(worker_data->temperature[cur_mag])) {
+							// set first valid value
+							worker_data->temperature[cur_mag] = new_temperature[cur_mag];
+
+						} else {
+							worker_data->temperature[cur_mag] = 0.5f * (worker_data->temperature[cur_mag] + new_temperature[cur_mag]);
+						}
+
 						worker_data->calibration_counter_total[cur_mag]++;
 					}
 				}
@@ -899,6 +912,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 					current_cal.set_offdiagonal(offdiag[cur_mag]);
 				}
 
+				current_cal.set_temperature(worker_data.temperature[cur_mag]);
+
 				current_cal.set_calibration_index(cur_mag);
 
 				current_cal.PrintStatus();
@@ -1009,6 +1024,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 				// use any existing scale and store the offset to the expected earth field
 				const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);
 				cal.set_offset(offset);
+				cal.set_temperature(mag.temperature);
 
 				// save new calibration
 				if (cal.ParametersSave()) {

src/modules/gyro_calibration/GyroCalibration.cpp

@@ -228,6 +228,8 @@ void GyroCalibration::Run()
 				const Vector3f old_offset{_gyro_calibration[gyro].offset()};
 
 				if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) {
+					_gyro_calibration[gyro].set_temperature(_temperature[gyro]);
+
 					calibration_updated = true;
 
 					PX4_INFO("gyro %d (%" PRIu32 ") updating calibration, [%.4f, %.4f, %.4f] -> [%.4f, %.4f, %.4f] %.1f°C",

src/modules/sensors/sensor_params_acc0.c

@@ -166,3 +166,14 @@ PARAM_DEFINE_FLOAT(CAL_ACC0_YSCALE, 1.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0f);
+
+/**
+ * Accelerometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC0_TEMP, NAN);

src/modules/sensors/sensor_params_acc1.c

@@ -166,3 +166,14 @@ PARAM_DEFINE_FLOAT(CAL_ACC1_YSCALE, 1.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0f);
+
+/**
+ * Accelerometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC1_TEMP, NAN);

src/modules/sensors/sensor_params_acc2.c

@@ -166,3 +166,14 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
+
+/**
+ * Accelerometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC2_TEMP, NAN);

src/modules/sensors/sensor_params_acc3.c

@@ -166,3 +166,14 @@ PARAM_DEFINE_FLOAT(CAL_ACC3_YSCALE, 1.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_ACC3_ZSCALE, 1.0f);
+
+/**
+ * Accelerometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_ACC3_TEMP, NAN);

src/modules/sensors/sensor_params_gyro0.c

@@ -139,3 +139,14 @@ PARAM_DEFINE_FLOAT(CAL_GYRO0_YOFF, 0.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f);
+
+/**
+ * Gyroscope calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO0_TEMP, NAN);

src/modules/sensors/sensor_params_gyro1.c

@@ -139,3 +139,14 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_YOFF, 0.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f);
+
+/**
+ * Gyroscope calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO1_TEMP, NAN);

src/modules/sensors/sensor_params_gyro2.c

@@ -139,3 +139,14 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_YOFF, 0.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f);
+
+/**
+ * Gyroscope calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO2_TEMP, NAN);

src/modules/sensors/sensor_params_gyro3.c

@@ -139,3 +139,14 @@ PARAM_DEFINE_FLOAT(CAL_GYRO3_YOFF, 0.0f);
  * @volatile
  */
 PARAM_DEFINE_FLOAT(CAL_GYRO3_ZOFF, 0.0f);
+
+/**
+ * Gyroscope calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_GYRO3_TEMP, NAN);

src/modules/sensors/sensor_params_mag0.c

@@ -235,3 +235,14 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_YCOMP, 0.0f);
 * @group Sensor Calibration
 */
 PARAM_DEFINE_FLOAT(CAL_MAG0_ZCOMP, 0.0f);
+
+/**
+ * Magnetometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG0_TEMP, NAN);

src/modules/sensors/sensor_params_mag1.c

@@ -235,3 +235,14 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_YCOMP, 0.0f);
 * @group Sensor Calibration
 */
 PARAM_DEFINE_FLOAT(CAL_MAG1_ZCOMP, 0.0f);
+
+/**
+ * Magnetometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG1_TEMP, NAN);

src/modules/sensors/sensor_params_mag2.c

@@ -235,3 +235,14 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_YCOMP, 0.0f);
 * @group Sensor Calibration
 */
 PARAM_DEFINE_FLOAT(CAL_MAG2_ZCOMP, 0.0f);
+
+/**
+ * Magnetometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG2_TEMP, NAN);

src/modules/sensors/sensor_params_mag3.c

@@ -235,3 +235,14 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_YCOMP, 0.0f);
 * @group Sensor Calibration
 */
 PARAM_DEFINE_FLOAT(CAL_MAG3_ZCOMP, 0.0f);
+
+/**
+ * Magnetometer calibration temperature
+ *
+ * Temperature during last calibration.
+ *
+ * @category system
+ * @group Sensor Calibration
+ * @volatile
+ */
+PARAM_DEFINE_FLOAT(CAL_MAG3_TEMP, NAN);

src/modules/sensors/sensors.cpp

@@ -319,22 +319,22 @@ int Sensors::parameters_update()
 	// mark all existing sensor calibrations active even if sensor is missing
 	// this preserves the calibration in the event of a parameter export while the sensor is missing
 	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-		uint32_t device_id_accel = calibration::GetCalibrationParam("ACC",  "ID", i);
-		uint32_t device_id_gyro  = calibration::GetCalibrationParam("GYRO", "ID", i);
-		uint32_t device_id_mag   = calibration::GetCalibrationParam("MAG",  "ID", i);
+		uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC",  "ID", i);
+		uint32_t device_id_gyro  = calibration::GetCalibrationParamInt32("GYRO", "ID", i);
+		uint32_t device_id_mag   = calibration::GetCalibrationParamInt32("MAG",  "ID", i);
 
 		if (device_id_accel != 0) {
-			bool external_accel = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0);
+			bool external_accel = (calibration::GetCalibrationParamInt32("ACC", "ROT", i) >= 0);
 			calibration::Accelerometer accel_cal(device_id_accel, external_accel);
 		}
 
 		if (device_id_gyro != 0) {
-			bool external_gyro = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0);
+			bool external_gyro = (calibration::GetCalibrationParamInt32("GYRO", "ROT", i) >= 0);
 			calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro);
 		}
 
 		if (device_id_mag != 0) {
-			bool external_mag = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0);
+			bool external_mag = (calibration::GetCalibrationParamInt32("MAG", "ROT", i) >= 0);
 			calibration::Magnetometer mag_cal(device_id_mag, external_mag);
 		}
 	}
@@ -343,21 +343,21 @@ int Sensors::parameters_update()
 	// this to done to eliminate differences in the active set of parameters before and after sensor calibration
 	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
 		if (orb_exists(ORB_ID(sensor_accel), i) == PX4_OK) {
-			bool external = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0);
+			bool external = (calibration::GetCalibrationParamInt32("ACC", "ROT", i) >= 0);
 			calibration::Accelerometer cal{0, external};
 			cal.set_calibration_index(i);
 			cal.ParametersUpdate();
 		}
 
 		if (orb_exists(ORB_ID(sensor_gyro), i) == PX4_OK) {
-			bool external = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0);
+			bool external = (calibration::GetCalibrationParamInt32("GYRO", "ROT", i) >= 0);
 			calibration::Gyroscope cal{0, external};
 			cal.set_calibration_index(i);
 			cal.ParametersUpdate();
 		}
 
 		if (orb_exists(ORB_ID(sensor_mag), i) == PX4_OK) {
-			bool external = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0);
+			bool external = (calibration::GetCalibrationParamInt32("MAG", "ROT", i) >= 0);
 			calibration::Magnetometer cal{0, external};
 			cal.set_calibration_index(i);
 			cal.ParametersUpdate();

src/modules/sensors/voted_sensors_update.cpp

@@ -96,7 +96,7 @@ void VotedSensorsUpdate::parametersUpdate()
 				// found matching CAL_ACCx_PRIO
 				int32_t accel_priority_old = _accel.priority_configured[uorb_index];
 
-				_accel.priority_configured[uorb_index] = calibration::GetCalibrationParam("ACC", "PRIO", accel_cal_index);
+				_accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index);
 
 				if (accel_priority_old != _accel.priority_configured[uorb_index]) {
 					if (_accel.priority_configured[uorb_index] == 0) {
@@ -119,7 +119,7 @@ void VotedSensorsUpdate::parametersUpdate()
 				// found matching CAL_GYROx_PRIO
 				int32_t gyro_priority_old = _gyro.priority_configured[uorb_index];
 
-				_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParam("GYRO", "PRIO", gyro_cal_index);
+				_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index);
 
 				if (gyro_priority_old != _gyro.priority_configured[uorb_index]) {
 					if (_gyro.priority_configured[uorb_index] == 0) {

src/modules/temperature_compensation/TemperatureCompensationModule.cpp

@@ -47,6 +47,11 @@ TemperatureCompensationModule::TemperatureCompensationModule() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
 	_loop_perf(perf_alloc(PC_ELAPSED, "temperature_compensation"))
 {
+	for (int i = 0; i < SENSOR_COUNT_MAX; i++) {
+		_corrections.accel_temperature[i] = NAN;
+		_corrections.gyro_temperature[i] = NAN;
+		_corrections.baro_temperature[i] = NAN;
+	}
 }
 
 TemperatureCompensationModule::~TemperatureCompensationModule()
@@ -128,6 +133,7 @@ void TemperatureCompensationModule::accelPoll()
 				if (_temperature_compensation.update_offsets_accel(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
 
 					_corrections.accel_device_ids[uorb_index] = report.device_id;
+					_corrections.accel_temperature[uorb_index] = report.temperature;
 					_corrections_changed = true;
 				}
 			}
@@ -150,6 +156,7 @@ void TemperatureCompensationModule::gyroPoll()
 				if (_temperature_compensation.update_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
 
 					_corrections.gyro_device_ids[uorb_index] = report.device_id;
+					_corrections.gyro_temperature[uorb_index] = report.temperature;
 					_corrections_changed = true;
 				}
 			}
@@ -172,6 +179,7 @@ void TemperatureCompensationModule::baroPoll()
 				if (_temperature_compensation.update_offsets_baro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
 
 					_corrections.baro_device_ids[uorb_index] = report.device_id;
+					_corrections.baro_temperature[uorb_index] = report.temperature;
 					_corrections_changed = true;
 				}
 			}