diff --git a/msg/sensor_correction.msg b/msg/sensor_correction.msg index 528ba32ed6..45ee23135d 100644 --- a/msg/sensor_correction.msg +++ b/msg/sensor_correction.msg @@ -7,6 +7,7 @@ uint64 timestamp # time since system start (microseconds) # Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] gyro_device_ids +float32[4] gyro_temperature float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s @@ -15,6 +16,7 @@ float32[3] gyro_offset_3 # gyro 3 XYZ offsets in the sensor frame in rad/s # Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] accel_device_ids +float32[4] accel_temperature float32[3] accel_offset_0 # accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s float32[3] accel_offset_1 # accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s float32[3] accel_offset_2 # accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s @@ -23,6 +25,7 @@ float32[3] accel_offset_3 # accelerometer 3 offsets in the FRD board frame XYZ-a # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] baro_device_ids +float32[4] baro_temperature float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in Pascals float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in Pascals float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in Pascals diff --git a/src/lib/parameters/templates/px4_parameters.hpp.jinja b/src/lib/parameters/templates/px4_parameters.hpp.jinja index 04ea82991a..a16046007d 100644 --- a/src/lib/parameters/templates/px4_parameters.hpp.jinja +++ b/src/lib/parameters/templates/px4_parameters.hpp.jinja @@ -1,5 +1,7 @@ {# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #} +#include // NAN + #include #include diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index b8ce46c655..d67f6372c5 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -169,7 +169,7 @@ void Accelerometer::ParametersUpdate() if (_calibration_index >= 0) { // CAL_ACCx_ROT - int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index); + int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); if (_external) { if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { @@ -194,7 +194,7 @@ void Accelerometer::ParametersUpdate() } // CAL_ACCx_PRIO - _priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index); + _priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index); if ((_priority < 0) || (_priority > 100)) { // reset to default, -1 is the uninitialized parameter value @@ -209,6 +209,9 @@ void Accelerometer::ParametersUpdate() _priority = new_priority; } + // CAL_ACCx_TEMP + set_temperature(GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index)); + // CAL_ACCx_OFF{X,Y,Z} set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); @@ -234,6 +237,7 @@ void Accelerometer::Reset() _scale = Vector3f{1.f, 1.f, 1.f}; _thermal_offset.zero(); + _temperature = NAN; _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; @@ -259,6 +263,8 @@ bool Accelerometer::ParametersSave() success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); } + success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature); + return success; } @@ -267,9 +273,11 @@ bool Accelerometer::ParametersSave() void Accelerometer::PrintStatus() { - PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), device_id(), - enabled(), - (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0), (double)_scale(1), (double)_scale(2)); + PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%.4f %.4f %.4f], scale: [%.4f %.4f %.4f], %.1f degC", + SensorString(), device_id(), enabled(), + (double)_offset(0), (double)_offset(1), (double)_offset(2), + (double)_scale(0), (double)_scale(1), (double)_scale(2), + (double)_temperature); if (_thermal_offset.norm() > 0.f) { PX4_INFO("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id, diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index c0a4ca667e..01fa97a9aa 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -65,6 +65,7 @@ public: bool set_offset(const matrix::Vector3f &offset); bool set_scale(const matrix::Vector3f &scale); void set_rotation(Rotation rotation); + void set_temperature(float temperature) { _temperature = temperature; }; uint8_t calibration_count() const { return _calibration_count; } uint32_t device_id() const { return _device_id; } @@ -99,6 +100,7 @@ private: matrix::Vector3f _offset; matrix::Vector3f _scale; matrix::Vector3f _thermal_offset; + float _temperature{NAN}; int8_t _calibration_index{-1}; uint32_t _device_id{0}; diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index 1417a7def1..d7d436a098 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -154,7 +154,7 @@ void Gyroscope::ParametersUpdate() if (_calibration_index >= 0) { // CAL_GYROx_ROT - int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index); + int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); if (_external) { if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { @@ -179,7 +179,7 @@ void Gyroscope::ParametersUpdate() } // CAL_GYROx_PRIO - _priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index); + _priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index); if ((_priority < 0) || (_priority > 100)) { // reset to default, -1 is the uninitialized parameter value @@ -194,6 +194,9 @@ void Gyroscope::ParametersUpdate() _priority = new_priority; } + // CAL_GYROx_TEMP + set_temperature(GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index)); + // CAL_GYROx_OFF{X,Y,Z} set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); @@ -215,6 +218,7 @@ void Gyroscope::Reset() _offset.zero(); _thermal_offset.zero(); + _temperature = NAN; _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; @@ -239,6 +243,8 @@ bool Gyroscope::ParametersSave() success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); } + success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature); + return success; } @@ -247,8 +253,10 @@ bool Gyroscope::ParametersSave() void Gyroscope::PrintStatus() { - PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%.4f %.4f %.4f]", SensorString(), device_id(), enabled(), - (double)_offset(0), (double)_offset(1), (double)_offset(2)); + PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%.4f %.4f %.4f], %.1f degC", + SensorString(), device_id(), enabled(), + (double)_offset(0), (double)_offset(1), (double)_offset(2), + (double)_temperature); if (_thermal_offset.norm() > 0.f) { PX4_INFO("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id, diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 0d249eedb6..1c386449a1 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); bool set_offset(const matrix::Vector3f &offset); void set_rotation(Rotation rotation); + void set_temperature(float temperature) { _temperature = temperature; }; uint8_t calibration_count() const { return _calibration_count; } uint32_t device_id() const { return _device_id; } @@ -102,6 +103,7 @@ private: matrix::Dcmf _rotation; matrix::Vector3f _offset; matrix::Vector3f _thermal_offset; + float _temperature{NAN}; int8_t _calibration_index{-1}; uint32_t _device_id{0}; diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 8dd2b7d028..524ee86717 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -154,7 +154,7 @@ void Magnetometer::ParametersUpdate() if (_calibration_index >= 0) { // CAL_MAGx_ROT - int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index); + int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); if (_external) { if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { @@ -179,7 +179,7 @@ void Magnetometer::ParametersUpdate() } // CAL_MAGx_PRIO - _priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index); + _priority = GetCalibrationParamInt32(SensorString(), "PRIO", _calibration_index); if ((_priority < 0) || (_priority > 100)) { // reset to default, -1 is the uninitialized parameter value @@ -194,6 +194,9 @@ void Magnetometer::ParametersUpdate() _priority = new_priority; } + // CAL_MAGx_TEMP + set_temperature(GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index)); + // CAL_MAGx_OFF{X,Y,Z} set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); @@ -227,6 +230,8 @@ void Magnetometer::Reset() _power_compensation.zero(); _power = 0.f; + _temperature = NAN; + _priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; _calibration_index = -1; @@ -258,6 +263,8 @@ bool Magnetometer::ParametersSave() success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); } + success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature); + return success; } @@ -267,17 +274,20 @@ bool Magnetometer::ParametersSave() void Magnetometer::PrintStatus() { if (external()) { - PX4_INFO("%s %" PRIu32 " EN: %d, offset: [% 05.3f % 05.3f % 05.3f], scale: [% 05.3f % 05.3f % 05.3f], External ROT: %d", + PX4_INFO("%s %" PRIu32 + " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, External ROT: %d", SensorString(), device_id(), enabled(), (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2), + (double)_temperature, rotation_enum()); } else { - PX4_INFO("%s %" PRIu32 " EN: %d, offset: [% 05.3f % 05.3f % 05.3f], scale: [% 05.3f % 05.3f % 05.3f], Internal", + PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Internal", SensorString(), device_id(), enabled(), (double)_offset(0), (double)_offset(1), (double)_offset(2), - (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2)); + (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2), + (double)_temperature); } #if defined(DEBUG_BUILD) diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index ce3ddb5f0f..677afe7e40 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -67,6 +67,7 @@ public: bool set_scale(const matrix::Vector3f &scale); bool set_offdiagonal(const matrix::Vector3f &offdiagonal); void set_rotation(Rotation rotation); + void set_temperature(float temperature) { _temperature = temperature; }; uint8_t calibration_count() const { return _calibration_count; } uint32_t device_id() const { return _device_id; } @@ -106,6 +107,7 @@ private: matrix::Matrix3f _scale; matrix::Vector3f _power_compensation; float _power{0.f}; + float _temperature{NAN}; int8_t _calibration_index{-1}; uint32_t _device_id{0}; diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index 571391c362..565636c926 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020,2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -76,7 +76,7 @@ int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id) return -1; } -int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance) +int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance) { // eg CAL_MAGn_ID/CAL_MAGn_ROT char str[20] {}; @@ -91,20 +91,19 @@ int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8 return value; } -bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value) +float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance) { + // eg CAL_MAGn_TEMP char str[20] {}; - - // eg CAL_MAGn_ID/CAL_MAGn_ROT sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type); - int ret = param_set_no_notification(param_find(str), &value); + float value = NAN; - if (ret != PX4_OK) { - PX4_ERR("failed to set %s = %" PRId32, str, value); + if (param_get(param_find(str), &value) != 0) { + PX4_ERR("failed to get %s", str); } - return ret == PX4_OK; + return value; } Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance) diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp index a2a8792a5d..f9ada74bc0 100644 --- a/src/lib/sensor_calibration/Utilities.hpp +++ b/src/lib/sensor_calibration/Utilities.hpp @@ -33,7 +33,11 @@ #pragma once +#include +#include #include +#include +#include #include namespace calibration @@ -56,7 +60,8 @@ int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id); * @param instance * @return int32_t The calibration value. */ -int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance); +int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance); +float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance); /** * @brief Set a single calibration paramter. @@ -67,7 +72,22 @@ int32_t GetCalibrationParam(const char *sensor_type, const char *cal_type, uint8 * @param value int32_t parameter value * @return true if the parameter name was valid and value saved successfully, false otherwise. */ -bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, int32_t value); +template +bool SetCalibrationParam(const char *sensor_type, const char *cal_type, uint8_t instance, T value) +{ + char str[20] {}; + + // eg CAL_MAGn_ID/CAL_MAGn_ROT + sprintf(str, "CAL_%s%u_%s", sensor_type, instance, cal_type); + + int ret = param_set_no_notification(param_find(str), &value); + + if (ret != PX4_OK) { + PX4_ERR("failed to set %s", str); + } + + return ret == PX4_OK; +} /** * @brief Get the Calibration Params Vector 3f object diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 6c9ebd6f7c..5907496d3f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/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(); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 3187feae37..ddba5a4232 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/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 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); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index dca7bb35ac..6900be8c04 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/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()) { diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp index cb6e8abecc..ced9d66c7f 100644 --- a/src/modules/gyro_calibration/GyroCalibration.cpp +++ b/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", diff --git a/src/modules/sensors/sensor_params_acc0.c b/src/modules/sensors/sensor_params_acc0.c index 992b8d0121..773e2d57fc 100644 --- a/src/modules/sensors/sensor_params_acc0.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_acc1.c b/src/modules/sensors/sensor_params_acc1.c index c8d17f4321..97953d6057 100644 --- a/src/modules/sensors/sensor_params_acc1.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_acc2.c b/src/modules/sensors/sensor_params_acc2.c index b97d6719ae..e309bd8d6e 100644 --- a/src/modules/sensors/sensor_params_acc2.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_acc3.c b/src/modules/sensors/sensor_params_acc3.c index 807b624d60..723545779d 100644 --- a/src/modules/sensors/sensor_params_acc3.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_gyro0.c b/src/modules/sensors/sensor_params_gyro0.c index 3b6693484f..a1cd917454 100644 --- a/src/modules/sensors/sensor_params_gyro0.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_gyro1.c b/src/modules/sensors/sensor_params_gyro1.c index 4545984191..ac939c2237 100644 --- a/src/modules/sensors/sensor_params_gyro1.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_gyro2.c b/src/modules/sensors/sensor_params_gyro2.c index 5f5ae348de..f6c97df594 100644 --- a/src/modules/sensors/sensor_params_gyro2.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_gyro3.c b/src/modules/sensors/sensor_params_gyro3.c index bfa1210e69..09265faf9e 100644 --- a/src/modules/sensors/sensor_params_gyro3.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_mag0.c b/src/modules/sensors/sensor_params_mag0.c index 170885da14..91e5dd34ab 100644 --- a/src/modules/sensors/sensor_params_mag0.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_mag1.c b/src/modules/sensors/sensor_params_mag1.c index 5ad5865ec6..f161b0d1b8 100644 --- a/src/modules/sensors/sensor_params_mag1.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_mag2.c b/src/modules/sensors/sensor_params_mag2.c index d2f7b5cdec..cb0b697fc5 100644 --- a/src/modules/sensors/sensor_params_mag2.c +++ b/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); diff --git a/src/modules/sensors/sensor_params_mag3.c b/src/modules/sensors/sensor_params_mag3.c index 56f070f129..42bd6c943e 100644 --- a/src/modules/sensors/sensor_params_mag3.c +++ b/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); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bb01dc8718..d44a622c18 100644 --- a/src/modules/sensors/sensors.cpp +++ b/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(); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 63ed4d2d63..35d1fe6cbf 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/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) { diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index 0d826f43a1..b7c21fe6a1 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/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; } }