sensors: preserve all valid calibration parameters even if sensor currently missing

- mark calibration slots active for first N sensors found
 - calibration procedure don't bother resetting unused slots

src/lib/sensor_calibration/Accelerometer.cpp

@@ -59,6 +59,11 @@ void Accelerometer::set_device_id(uint32_t device_id, bool external)
 	if (_device_id != device_id || _external != external) {
 		set_external(external);
 		_device_id = device_id;
+
+		if (_device_id != 0) {
+			_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+		}
+
 		ParametersUpdate();
 		SensorCorrectionsUpdate(true);
 	}
@@ -161,13 +166,6 @@ void Accelerometer::set_rotation(Rotation rotation)
 
 void Accelerometer::ParametersUpdate()
 {
-	if (_device_id == 0) {
-		Reset();
-		return;
-	}
-
-	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-
 	if (_calibration_index >= 0) {
 
 		// CAL_ACCx_ROT
@@ -234,6 +232,7 @@ void Accelerometer::Reset()
 
 	_offset.zero();
 	_scale = Vector3f{1.f, 1.f, 1.f};
+
 	_thermal_offset.zero();
 
 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;

src/lib/sensor_calibration/Gyroscope.cpp

@@ -59,6 +59,11 @@ void Gyroscope::set_device_id(uint32_t device_id, bool external)
 	if (_device_id != device_id || _external != external) {
 		set_external(external);
 		_device_id = device_id;
+
+		if (_device_id != 0) {
+			_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+		}
+
 		ParametersUpdate();
 		SensorCorrectionsUpdate(true);
 	}
@@ -146,13 +151,6 @@ void Gyroscope::set_rotation(Rotation rotation)
 
 void Gyroscope::ParametersUpdate()
 {
-	if (_device_id == 0) {
-		Reset();
-		return;
-	}
-
-	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-
 	if (_calibration_index >= 0) {
 
 		// CAL_GYROx_ROT
@@ -215,6 +213,7 @@ void Gyroscope::Reset()
 	}
 
 	_offset.zero();
+
 	_thermal_offset.zero();
 
 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;

src/lib/sensor_calibration/Magnetometer.cpp

@@ -59,6 +59,11 @@ void Magnetometer::set_device_id(uint32_t device_id, bool external)
 	if (_device_id != device_id || _external != external) {
 		set_external(external);
 		_device_id = device_id;
+
+		if (_device_id != 0) {
+			_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+		}
+
 		ParametersUpdate();
 	}
 }
@@ -146,13 +151,6 @@ void Magnetometer::set_rotation(Rotation rotation)
 
 void Magnetometer::ParametersUpdate()
 {
-	if (_device_id == 0) {
-		Reset();
-		return;
-	}
-
-	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-
 	if (_calibration_index >= 0) {
 
 		// CAL_MAGx_ROT

src/modules/commander/accelerometer_calibration.cpp

@@ -418,17 +418,17 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 				accel_T_rotated.print();
 #endif // DEBUD_BUILD
 				calibrations[i].PrintStatus();
-			}
 
-			// save all calibrations including empty slots
-			if (calibrations[i].ParametersSave()) {
-				param_save = true;
-				failed = false;
 
-			} else {
-				failed = true;
-				calibration_log_critical(mavlink_log_pub, "calibration save failed");
-				break;
+				if (calibrations[i].ParametersSave()) {
+					param_save = true;
+					failed = false;
+
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, "calibration save failed");
+					break;
+				}
 			}
 		}
 

src/modules/commander/gyro_calibration.cpp

@@ -260,22 +260,19 @@ 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_calibration_index(uorb_index);
+
 				calibration.PrintStatus();
 
-			} else {
-				calibration.Reset();
-			}
+				if (calibration.ParametersSave()) {
+					param_save = true;
+					failed = false;
 
-			calibration.set_calibration_index(uorb_index);
-
-			if (calibration.ParametersSave()) {
-				param_save = true;
-				failed = false;
-
-			} else {
-				failed = true;
-				calibration_log_critical(mavlink_log_pub, "calibration save failed");
-				break;
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, "calibration save failed");
+					break;
+				}
 			}
 		}
 

src/modules/commander/mag_calibration.cpp

@@ -897,22 +897,19 @@ 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_calibration_index(cur_mag);
+
 				current_cal.PrintStatus();
 
-			} else {
-				current_cal.Reset();
-			}
+				if (current_cal.ParametersSave()) {
+					param_save = true;
+					failed = false;
 
-			current_cal.set_calibration_index(cur_mag);
-
-			if (current_cal.ParametersSave()) {
-				param_save = true;
-				failed = false;
-
-			} else {
-				failed = true;
-				calibration_log_critical(mavlink_log_pub, "calibration save failed");
-				break;
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, "calibration save failed");
+					break;
+				}
 			}
 		}
 
@@ -1000,7 +997,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 			sensor_mag_s mag{};
 			mag_sub.copy(&mag);
 
-			if (mag_sub.advertised() && (mag.timestamp != 0)) {
+			if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) {
 
 				calibration::Magnetometer cal{mag.device_id, mag.is_external};
 

src/modules/sensors/sensors.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -48,6 +48,7 @@
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Utilities.hpp>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
@@ -179,6 +180,12 @@ private:
 
 	VehicleIMU      *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
 
+	uint8_t _n_accel{0};
+	uint8_t _n_baro{0};
+	uint8_t _n_gps{0};
+	uint8_t _n_gyro{0};
+	uint8_t _n_mag{0};
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -308,6 +315,54 @@ int Sensors::parameters_update()
 
 	_voted_sensors_update.parametersUpdate();
 
+	// 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);
+
+		if (device_id_accel != 0) {
+			bool external_accel = (calibration::GetCalibrationParam("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);
+			calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro);
+		}
+
+		if (device_id_mag != 0) {
+			bool external_mag = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0);
+			calibration::Magnetometer mag_cal(device_id_mag, external_mag);
+		}
+	}
+
+	// ensure calibration slots are active for the number of sensors currently available
+	// 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);
+			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);
+			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);
+			calibration::Magnetometer cal{0, external};
+			cal.set_calibration_index(i);
+			cal.ParametersUpdate();
+		}
+	}
+
 	return PX4_OK;
 }
 
@@ -611,11 +666,28 @@ void Sensors::Run()
 	// keep adding sensors as long as we are not armed,
 	// when not adding sensors poll for param updates
 	if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) {
-		_voted_sensors_update.initializeSensors();
-		InitializeVehicleAirData();
-		InitializeVehicleIMU();
-		InitializeVehicleGPSPosition();
-		InitializeVehicleMagnetometer();
+
+		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
+		const int n_baro  = orb_group_count(ORB_ID(sensor_baro));
+		const int n_gps   = orb_group_count(ORB_ID(sensor_gps));
+		const int n_gyro  = orb_group_count(ORB_ID(sensor_gyro));
+		const int n_mag   = orb_group_count(ORB_ID(sensor_mag));
+
+		if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) {
+			_n_accel = n_accel;
+			_n_baro = n_baro;
+			_n_gps = n_gps;
+			_n_gyro = n_gyro;
+			_n_mag = n_mag;
+			parameters_update();
+
+			_voted_sensors_update.initializeSensors();
+			InitializeVehicleAirData();
+			InitializeVehicleIMU();
+			InitializeVehicleGPSPosition();
+			InitializeVehicleMagnetometer();
+		}
+
 		_last_config_update = hrt_absolute_time();
 
 	} else {