accel/gyro/mag calibration set default priority uninitialized

- this allows the default priority to be set differently for internal/external sensors
 - accel and gyro initialize default priority like mag in preparation for fully supporting external sensors
 - fixes #15652

src/lib/sensor_calibration/Accelerometer.cpp

@@ -48,21 +48,42 @@ Accelerometer::Accelerometer()
 	Reset();
 }
 
-Accelerometer::Accelerometer(uint32_t device_id)
+Accelerometer::Accelerometer(uint32_t device_id, bool external)
 {
 	Reset();
-	set_device_id(device_id);
+	set_device_id(device_id, external);
 }
 
-void Accelerometer::set_device_id(uint32_t device_id)
+void Accelerometer::set_device_id(uint32_t device_id, bool external)
 {
-	if (_device_id != device_id) {
+	if (_device_id != device_id || _external != external) {
+		set_external(external);
 		_device_id = device_id;
 		ParametersUpdate();
 		SensorCorrectionsUpdate(true);
 	}
 }
 
+void Accelerometer::set_external(bool external)
+{
+	// update priority default appropriately if not set
+	if (_calibration_index < 0 || _priority < 0) {
+		if ((_priority < 0) || (_priority > 100)) {
+			_priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+
+		} else if (!_external && external && (_priority == DEFAULT_PRIORITY)) {
+			// internal -> external
+			_priority = DEFAULT_EXTERNAL_PRIORITY;
+
+		} else if (_external && !external && (_priority == DEFAULT_EXTERNAL_PRIORITY)) {
+			// external -> internal
+			_priority = DEFAULT_PRIORITY;
+		}
+	}
+
+	_external = external;
+}
+
 void Accelerometer::SensorCorrectionsUpdate(bool force)
 {
 	// check if the selected sensor has updated

src/lib/sensor_calibration/Accelerometer.hpp

@@ -53,15 +53,15 @@ public:
 	static constexpr const char *SensorString() { return "ACC"; }
 
 	Accelerometer();
-	explicit Accelerometer(uint32_t device_id);
+	explicit Accelerometer(uint32_t device_id, bool external = false);
 
 	~Accelerometer() = default;
 
 	void PrintStatus();
 
 	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
-	void set_device_id(uint32_t device_id);
-	void set_external(bool external = true) { _external = external; }
+	void set_device_id(uint32_t device_id, bool external = false);
+	void set_external(bool external = true);
 	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
 	void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
 
@@ -95,7 +95,7 @@ private:
 
 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
-	int32_t _priority{DEFAULT_PRIORITY};
+	int32_t _priority{-1};
 
 	bool _external{false};
 };

src/lib/sensor_calibration/Gyroscope.cpp

@@ -48,21 +48,42 @@ Gyroscope::Gyroscope()
 	Reset();
 }
 
-Gyroscope::Gyroscope(uint32_t device_id)
+Gyroscope::Gyroscope(uint32_t device_id, bool external)
 {
 	Reset();
-	set_device_id(device_id);
+	set_device_id(device_id, external);
 }
 
-void Gyroscope::set_device_id(uint32_t device_id)
+void Gyroscope::set_device_id(uint32_t device_id, bool external)
 {
-	if (_device_id != device_id) {
+	if (_device_id != device_id || _external != external) {
+		set_external(external);
 		_device_id = device_id;
 		ParametersUpdate();
 		SensorCorrectionsUpdate(true);
 	}
 }
 
+void Gyroscope::set_external(bool external)
+{
+	// update priority default appropriately if not set
+	if (_calibration_index < 0 || _priority < 0) {
+		if ((_priority < 0) || (_priority > 100)) {
+			_priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
+
+		} else if (!_external && external && (_priority == DEFAULT_PRIORITY)) {
+			// internal -> external
+			_priority = DEFAULT_EXTERNAL_PRIORITY;
+
+		} else if (_external && !external && (_priority == DEFAULT_EXTERNAL_PRIORITY)) {
+			// external -> internal
+			_priority = DEFAULT_PRIORITY;
+		}
+	}
+
+	_external = external;
+}
+
 void Gyroscope::SensorCorrectionsUpdate(bool force)
 {
 	// check if the selected sensor has updated

src/lib/sensor_calibration/Gyroscope.hpp

@@ -53,15 +53,15 @@ public:
 	static constexpr const char *SensorString() { return "GYRO"; }
 
 	Gyroscope();
-	explicit Gyroscope(uint32_t device_id);
+	explicit Gyroscope(uint32_t device_id, bool external = false);
 
 	~Gyroscope() = default;
 
 	void PrintStatus();
 
 	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
-	void set_device_id(uint32_t device_id);
-	void set_external(bool external = true) { _external = external; }
+	void set_device_id(uint32_t device_id, bool external = false);
+	void set_external(bool external = true);
 	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
 
 	uint32_t device_id() const { return _device_id; }
@@ -93,7 +93,7 @@ private:
 
 	int8_t _calibration_index{-1};
 	uint32_t _device_id{0};
-	int32_t _priority{DEFAULT_PRIORITY};
+	int32_t _priority{-1};
 
 	bool _external{false};
 };

src/lib/sensor_calibration/Magnetometer.cpp

@@ -50,14 +50,14 @@ Magnetometer::Magnetometer()
 
 Magnetometer::Magnetometer(uint32_t device_id, bool external)
 {
-	set_external(external);
 	Reset();
-	set_device_id(device_id);
+	set_device_id(device_id, external);
 }
 
-void Magnetometer::set_device_id(uint32_t device_id)
+void Magnetometer::set_device_id(uint32_t device_id, bool external)
 {
-	if (_device_id != device_id) {
+	if (_device_id != device_id || _external != external) {
+		set_external(external);
 		_device_id = device_id;
 		ParametersUpdate();
 	}
@@ -66,7 +66,7 @@ void Magnetometer::set_device_id(uint32_t device_id)
 void Magnetometer::set_external(bool external)
 {
 	// update priority default appropriately if not set
-	if (_calibration_index < 0) {
+	if (_calibration_index < 0 || _priority < 0) {
 		if ((_priority < 0) || (_priority > 100)) {
 			_priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 

src/lib/sensor_calibration/Magnetometer.hpp

@@ -61,7 +61,7 @@ public:
 	void PrintStatus();
 
 	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
-	void set_device_id(uint32_t device_id);
+	void set_device_id(uint32_t device_id, bool external = false);
 	void set_external(bool external = true);
 	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
 	void set_scale(const matrix::Vector3f &scale);

src/modules/commander/mag_calibration.cpp

@@ -451,8 +451,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 		uORB::SubscriptionData<sensor_mag_s> mag_sub{ORB_ID(sensor_mag), cur_mag};
 
 		if (mag_sub.advertised() && (mag_sub.get().device_id != 0) && (mag_sub.get().timestamp > 0)) {
-			worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id);
-			worker_data.calibration[cur_mag].set_external(mag_sub.get().is_external);
+			worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id, mag_sub.get().is_external);
 		}
 
 		// reset calibration index to match uORB numbering

src/modules/sensors/sensor_params_acc0.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
 /**
  * Accelerometer 0 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_ACC0_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_ACC0_PRIO, -1);
 
 /**
  * Accelerometer X-axis offset

src/modules/sensors/sensor_params_acc1.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
 /**
  * Accelerometer 1 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_ACC1_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_ACC1_PRIO, -1);
 
 /**
  * Accelerometer X-axis offset

src/modules/sensors/sensor_params_acc2.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
 /**
  * Accelerometer 2 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_ACC2_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_ACC2_PRIO, -1);
 
 /**
  * Accelerometer X-axis offset

src/modules/sensors/sensor_params_gyro0.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
 /**
  * Gyro 0 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, -1);
 
 /**
  * Gyro X-axis offset

src/modules/sensors/sensor_params_gyro1.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
 /**
  * Gyro 1 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, -1);
 
 /**
  * Gyro X-axis offset

src/modules/sensors/sensor_params_gyro2.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
 /**
  * Gyro 2 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, -1);
 
 /**
  * Gyro X-axis offset

src/modules/sensors/sensor_params_mag0.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
 /**
  * Mag 0 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG0_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_MAG0_PRIO, -1);
 
 /**
  * Rotation of magnetometer 0 relative to airframe.

src/modules/sensors/sensor_params_mag1.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
 /**
  * Mag 1 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG1_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1);
 
 /**
  * Rotation of magnetometer 1 relative to airframe.

src/modules/sensors/sensor_params_mag2.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
 /**
  * Mag 2 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG2_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1);
 
 /**
  * Rotation of magnetometer 2 relative to airframe.

src/modules/sensors/sensor_params_mag3.c

@@ -42,6 +42,7 @@ PARAM_DEFINE_INT32(CAL_MAG3_ID, 0);
 /**
  * Mag 3 priority.
  *
+ * @value -1  Uninitialized
  * @value 0   Disabled
  * @value 1   Min
  * @value 25  Low
@@ -52,7 +53,7 @@ PARAM_DEFINE_INT32(CAL_MAG3_ID, 0);
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG3_PRIO, 50);
+PARAM_DEFINE_INT32(CAL_MAG3_PRIO, -1);
 
 /**
  * Rotation of magnetometer 3 relative to airframe.

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -224,8 +224,7 @@ void VehicleMagnetometer::Run()
 				updated[uorb_index] = true;
 
 				if (_calibration[uorb_index].device_id() != report.device_id) {
-					_calibration[uorb_index].set_external(report.is_external);
-					_calibration[uorb_index].set_device_id(report.device_id);
+					_calibration[uorb_index].set_device_id(report.device_id, report.is_external);
 					_priority[uorb_index] = _calibration[uorb_index].priority();
 				}