fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 18:53:33 +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-08-15 11:19:24 -04:00
parent a76bcd3e01
commit ff39e27e2d
29 changed files with 295 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Accelerometer.cpp
+14 -6
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,
src/lib/sensor_calibration/Accelerometer.hpp
+2
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};
src/lib/sensor_calibration/Gyroscope.cpp
+13 -5
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,
src/lib/sensor_calibration/Gyroscope.hpp
+2
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};
src/lib/sensor_calibration/Magnetometer.cpp
+16 -6
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)
src/lib/sensor_calibration/Magnetometer.hpp
+2
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};
src/lib/sensor_calibration/Utilities.cpp
+8 -9
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)
src/lib/sensor_calibration/Utilities.hpp
+22 -2
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