diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg index f5e8da10a6..c277d5661a 100644 --- a/msg/vehicle_imu.msg +++ b/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. diff --git a/msg/vehicle_magnetometer.msg b/msg/vehicle_magnetometer.msg index e279d16bba..981814b259 100644 --- a/msg/vehicle_magnetometer.msg +++ b/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. diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index 66a8a536b2..1d5f4411df 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/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() diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index f24ad538ae..eb5ea27164 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/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 diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index 669e3fb581..a2144d8e2f 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/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() diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index e25707a185..1180cabed5 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/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 diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 401fe7ed8e..7c88bda7ce 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/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() diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 5cbafcee77..13ddc83eff 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/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 diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index b0d64ee255..a4287537cc 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/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); } diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index a1fb0f3e71..838122f0ad 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/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();