vehicle_imu/vehicle_magnetometer add calibration indicator to message

- vehicle_imu/vehicle_magnetometer add monotonically increasing `calibration_count` field so that downstream subscribers are aware of calibration changes

msg/vehicle_imu.msg

@@ -15,3 +15,5 @@ uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
 uint8 delta_velocity_clipping   # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
+
+uint8 calibration_count         # Calibration changed counter. Monotonically increases whenever calibration changes.

msg/vehicle_magnetometer.msg

@@ -6,3 +6,5 @@ uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
 uint32 device_id            # unique device ID for the selected magnetometer
 
 float32[3] magnetometer_ga  # Magnetic field in NED body frame, in Gauss
+
+uint8 calibration_count     # Calibration changed counter. Monotonically increases whenever calibration changes.

src/lib/sensor_calibration/Accelerometer.cpp

@@ -149,11 +149,27 @@ void Accelerometer::ParametersUpdate()
 			_priority = DEFAULT_PRIORITY;
 		}
 
+		bool calibration_changed = false;
+
 		// CAL_ACCx_OFF{X,Y,Z}
-		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+		const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+
+		if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
+			calibration_changed = true;
+			_offset = offset;
+		}
 
 		// CAL_ACCx_SCALE{X,Y,Z}
-		_scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
+		const Vector3f scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
+
+		if (Vector3f(_scale - scale).norm_squared() > 0.001f * 0.001f) {
+			calibration_changed = true;
+			_scale = scale;
+		}
+
+		if (calibration_changed) {
+			_calibration_count++;
+		}
 
 	} else {
 		Reset();
@@ -170,6 +186,8 @@ void Accelerometer::Reset()
 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 
 	_calibration_index = -1;
+
+	_calibration_count = 0;
 }
 
 bool Accelerometer::ParametersSave()

src/lib/sensor_calibration/Accelerometer.hpp

@@ -65,6 +65,7 @@ public:
 	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
 	void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
 
+	uint8_t calibration_count() const { return _calibration_count; }
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }
@@ -98,5 +99,7 @@ private:
 	int32_t _priority{-1};
 
 	bool _external{false};
+
+	uint8_t _calibration_count{0};
 };
 } // namespace calibration

src/lib/sensor_calibration/Gyroscope.cpp

@@ -149,8 +149,19 @@ void Gyroscope::ParametersUpdate()
 			_priority = DEFAULT_PRIORITY;
 		}
 
+		bool calibration_changed = false;
+
 		// CAL_GYROx_OFF{X,Y,Z}
-		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+		const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+
+		if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
+			calibration_changed = true;
+			_offset = offset;
+		}
+
+		if (calibration_changed) {
+			_calibration_count++;
+		}
 
 	} else {
 		Reset();
@@ -166,6 +177,8 @@ void Gyroscope::Reset()
 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 
 	_calibration_index = -1;
+
+	_calibration_count = 0;
 }
 
 bool Gyroscope::ParametersSave()

src/lib/sensor_calibration/Gyroscope.hpp

@@ -64,6 +64,7 @@ public:
 	void set_external(bool external = true);
 	void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
 
+	uint8_t calibration_count() const { return _calibration_count; }
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }
@@ -96,5 +97,7 @@ private:
 	int32_t _priority{-1};
 
 	bool _external{false};
+
+	uint8_t _calibration_count{0};
 };
 } // namespace calibration

src/lib/sensor_calibration/Magnetometer.cpp

@@ -157,21 +157,42 @@ void Magnetometer::ParametersUpdate()
 			_priority = new_priority;
 		}
 
+		bool calibration_changed = false;
+
 		// CAL_MAGx_OFF{X,Y,Z}
-		_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+		const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
+
+		if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
+			calibration_changed = true;
+			_offset = offset;
+		}
 
 		// CAL_MAGx_SCALE{X,Y,Z}
 		const Vector3f diag = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
 
+		if (Vector3f(_scale.diag() - diag).norm_squared() > 0.001f * 0.001f) {
+			calibration_changed = true;
+		}
+
 		// CAL_MAGx_ODIAG{X,Y,Z}
 		const Vector3f offdiag = GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index);
 
-		float scale[9] {
-			diag(0),    offdiag(0), offdiag(1),
-			offdiag(0),    diag(1), offdiag(2),
-			offdiag(1), offdiag(2),    diag(2)
-		};
-		_scale = Matrix3f{scale};
+		if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiag).norm_squared() > 0.001f * 0.001f) {
+			calibration_changed = true;
+		}
+
+		if (calibration_changed) {
+
+			float scale[9] {
+				diag(0),    offdiag(0), offdiag(1),
+				offdiag(0),    diag(1), offdiag(2),
+				offdiag(1), offdiag(2),    diag(2)
+			};
+			_scale = Matrix3f{scale};
+
+			_calibration_count++;
+		}
+
 
 		// CAL_MAGx_COMP{X,Y,Z}
 		_power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index);
@@ -194,6 +215,8 @@ void Magnetometer::Reset()
 	_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
 
 	_calibration_index = -1;
+
+	_calibration_count = 0;
 }
 
 bool Magnetometer::ParametersSave()

src/lib/sensor_calibration/Magnetometer.hpp

@@ -68,6 +68,7 @@ public:
 	void set_offdiagonal(const matrix::Vector3f &offdiagonal);
 	void set_rotation(Rotation rotation);
 
+	uint8_t calibration_count() const { return _calibration_count; }
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }
@@ -105,5 +106,7 @@ private:
 	int32_t _priority{-1};
 
 	bool _external{false};
+
+	uint8_t _calibration_count{0};
 };
 } // namespace calibration

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -379,6 +379,7 @@ void VehicleIMU::Run()
 				imu.delta_angle_dt = gyro_integral_dt;
 				imu.delta_velocity_dt = accel_integral_dt;
 				imu.delta_velocity_clipping = _delta_velocity_clipping;
+				imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count();
 				imu.timestamp = hrt_absolute_time();
 				_vehicle_imu_pub.publish(imu);
 			}

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -357,6 +357,7 @@ void VehicleMagnetometer::Publish(uint8_t instance, bool multi)
 		out.timestamp_sample = timestamp_sample;
 		out.device_id = _calibration[instance].device_id();
 		magnetometer_data.copyTo(out.magnetometer_ga);
+		out.calibration_count = _calibration[instance].calibration_count();
 
 		out.timestamp = hrt_absolute_time();