fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor calibration: save temperature at calibration time for monitoring

Browse Source
This commit is contained in:
Daniel Agar 2021-04-01 11:24:53 -04:00
parent a76bcd3e01
commit ff39e27e2d
29 changed files with 295 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/sensor_correction.msg
View File

@ -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

2
src/lib/parameters/templates/px4_parameters.hpp.jinja
View File

@ -1,5 +1,7 @@
{# jinja syntax: http://jinja.pocoo.org/docs/2.9/templates/ #}
#include <math.h> // NAN
#include <stdint.h>
#include <parameters/param.h>

20
src/lib/sensor_calibration/Accelerometer.cpp
View File

@ -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,

2
src/lib/sensor_calibration/Accelerometer.hpp
View File

@ -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};

18
src/lib/sensor_calibration/Gyroscope.cpp
View File

@ -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,

2
src/lib/sensor_calibration/Gyroscope.hpp
View File

@ -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};

22
src/lib/sensor_calibration/Magnetometer.cpp
View File

@ -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)

2
src/lib/sensor_calibration/Magnetometer.hpp
View File

@ -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};

17
src/lib/sensor_calibration/Utilities.cpp
View File

@ -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)

24
src/lib/sensor_calibration/Utilities.hpp
View File

@ -33,7 +33,11 @@
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <lib/conversion/rotation.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <matrix/math.hpp>
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<typename T>
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

32
src/modules/commander/accelerometer_calibration.cpp
View File

@ -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();

12
src/modules/commander/gyro_calibration.cpp
View File

@ -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<float, 9> 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);

16
src/modules/commander/mag_calibration.cpp
View File

@ -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()) {

2
src/modules/gyro_calibration/GyroCalibration.cpp
View File

@ -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",

11
src/modules/sensors/sensor_params_acc0.c
View File

@ -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);

11
src/modules/sensors/sensor_params_acc1.c
View File

@ -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);

11
src/modules/sensors/sensor_params_acc2.c
View File

@ -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);

11
src/modules/sensors/sensor_params_acc3.c
View File

@ -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);

11
src/modules/sensors/sensor_params_gyro0.c
View File

@ -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);

11
src/modules/sensors/sensor_params_gyro1.c
View File

@ -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);

11
src/modules/sensors/sensor_params_gyro2.c
View File

@ -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);

11
src/modules/sensors/sensor_params_gyro3.c
View File

@ -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);

11
src/modules/sensors/sensor_params_mag0.c
View File

@ -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);

11
src/modules/sensors/sensor_params_mag1.c
View File

@ -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);

11
src/modules/sensors/sensor_params_mag2.c
View File

@ -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);

11
src/modules/sensors/sensor_params_mag3.c
View File

@ -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);

18
src/modules/sensors/sensors.cpp
View File

@ -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();

4
src/modules/sensors/voted_sensors_update.cpp
View File

@ -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) {

8
src/modules/temperature_compensation/TemperatureCompensationModule.cpp
View File

@ -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;
}
}