mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-26 19:53:33 +08:00
sensor calibration delete temperature (CAL_ACCx_TEMP, CAL_GYROx_TEMP, CAL_MAGx_TEMP)
- this was an experiment to casually monitor sensor offsets relative to temperature, but now that all calibration offsets can be adjusted post-flight the stored temperature can be misleading - deleting to save a little bit of flash (and storing the temperature wasn't useful)
This commit is contained in:
@@ -207,16 +207,6 @@ bool Accelerometer::ParametersLoad()
|
|||||||
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
||||||
}
|
}
|
||||||
|
|
||||||
// CAL_ACCx_TEMP
|
|
||||||
float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
|
|
||||||
|
|
||||||
if (cal_temp > TEMPERATURE_INVALID) {
|
|
||||||
set_temperature(cal_temp);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
set_temperature(NAN);
|
|
||||||
}
|
|
||||||
|
|
||||||
// CAL_ACCx_OFF{X,Y,Z}
|
// CAL_ACCx_OFF{X,Y,Z}
|
||||||
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
|
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
|
||||||
|
|
||||||
@@ -243,7 +233,6 @@ void Accelerometer::Reset()
|
|||||||
_scale = Vector3f{1.f, 1.f, 1.f};
|
_scale = Vector3f{1.f, 1.f, 1.f};
|
||||||
|
|
||||||
_thermal_offset.zero();
|
_thermal_offset.zero();
|
||||||
_temperature = NAN;
|
|
||||||
|
|
||||||
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
||||||
|
|
||||||
@@ -285,13 +274,6 @@ bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
|
|||||||
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
|
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
|
||||||
}
|
}
|
||||||
|
|
||||||
if (PX4_ISFINITE(_temperature)) {
|
|
||||||
success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
|
|
||||||
}
|
|
||||||
|
|
||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -302,20 +284,18 @@ void Accelerometer::PrintStatus()
|
|||||||
{
|
{
|
||||||
if (external()) {
|
if (external()) {
|
||||||
PX4_INFO_RAW("%s %" PRIu32
|
PX4_INFO_RAW("%s %" PRIu32
|
||||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d\n",
|
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
|
||||||
SensorString(), device_id(), enabled(),
|
SensorString(), device_id(), enabled(),
|
||||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
||||||
(double)_scale(0), (double)_scale(1), (double)_scale(2),
|
(double)_scale(0), (double)_scale(1), (double)_scale(2),
|
||||||
(double)_temperature,
|
|
||||||
rotation_enum());
|
rotation_enum());
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_INFO_RAW("%s %" PRIu32
|
PX4_INFO_RAW("%s %" PRIu32
|
||||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Internal\n",
|
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
|
||||||
SensorString(), device_id(), enabled(),
|
SensorString(), device_id(), enabled(),
|
||||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
||||||
(double)_scale(0), (double)_scale(1), (double)_scale(2),
|
(double)_scale(0), (double)_scale(1), (double)_scale(2));
|
||||||
(double)_temperature);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_thermal_offset.norm() > 0.f) {
|
if (_thermal_offset.norm() > 0.f) {
|
||||||
|
|||||||
@@ -64,7 +64,6 @@ public:
|
|||||||
bool set_offset(const matrix::Vector3f &offset);
|
bool set_offset(const matrix::Vector3f &offset);
|
||||||
bool set_scale(const matrix::Vector3f &scale);
|
bool set_scale(const matrix::Vector3f &scale);
|
||||||
void set_rotation(Rotation rotation);
|
void set_rotation(Rotation rotation);
|
||||||
void set_temperature(float temperature) { _temperature = temperature; };
|
|
||||||
|
|
||||||
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
|
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
|
||||||
uint8_t calibration_count() const { return _calibration_count; }
|
uint8_t calibration_count() const { return _calibration_count; }
|
||||||
@@ -100,8 +99,6 @@ public:
|
|||||||
void SensorCorrectionsUpdate(bool force = false);
|
void SensorCorrectionsUpdate(bool force = false);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static constexpr float TEMPERATURE_INVALID = -1000.f;
|
|
||||||
|
|
||||||
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
|
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
|
||||||
|
|
||||||
Rotation _rotation_enum{ROTATION_NONE};
|
Rotation _rotation_enum{ROTATION_NONE};
|
||||||
@@ -110,7 +107,6 @@ private:
|
|||||||
matrix::Vector3f _offset;
|
matrix::Vector3f _offset;
|
||||||
matrix::Vector3f _scale;
|
matrix::Vector3f _scale;
|
||||||
matrix::Vector3f _thermal_offset;
|
matrix::Vector3f _thermal_offset;
|
||||||
float _temperature{NAN};
|
|
||||||
|
|
||||||
int8_t _calibration_index{-1};
|
int8_t _calibration_index{-1};
|
||||||
uint32_t _device_id{0};
|
uint32_t _device_id{0};
|
||||||
|
|||||||
@@ -192,16 +192,6 @@ bool Gyroscope::ParametersLoad()
|
|||||||
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
||||||
}
|
}
|
||||||
|
|
||||||
// CAL_GYROx_TEMP
|
|
||||||
float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
|
|
||||||
|
|
||||||
if (cal_temp > TEMPERATURE_INVALID) {
|
|
||||||
set_temperature(cal_temp);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
set_temperature(NAN);
|
|
||||||
}
|
|
||||||
|
|
||||||
// CAL_GYROx_OFF{X,Y,Z}
|
// CAL_GYROx_OFF{X,Y,Z}
|
||||||
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
|
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
|
||||||
|
|
||||||
@@ -224,7 +214,6 @@ void Gyroscope::Reset()
|
|||||||
_offset.zero();
|
_offset.zero();
|
||||||
|
|
||||||
_thermal_offset.zero();
|
_thermal_offset.zero();
|
||||||
_temperature = NAN;
|
|
||||||
|
|
||||||
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
||||||
|
|
||||||
@@ -265,13 +254,6 @@ bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
|
|||||||
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
|
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
|
||||||
}
|
}
|
||||||
|
|
||||||
if (PX4_ISFINITE(_temperature)) {
|
|
||||||
success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
|
|
||||||
}
|
|
||||||
|
|
||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -282,17 +264,15 @@ void Gyroscope::PrintStatus()
|
|||||||
{
|
{
|
||||||
if (external()) {
|
if (external()) {
|
||||||
PX4_INFO_RAW("%s %" PRIu32
|
PX4_INFO_RAW("%s %" PRIu32
|
||||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d\n",
|
" EN: %d, offset: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
|
||||||
SensorString(), device_id(), enabled(),
|
SensorString(), device_id(), enabled(),
|
||||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
||||||
(double)_temperature,
|
|
||||||
rotation_enum());
|
rotation_enum());
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], %.1f degC, Internal\n",
|
PX4_INFO_RAW("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], Internal\n",
|
||||||
SensorString(), device_id(), enabled(),
|
SensorString(), device_id(), enabled(),
|
||||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
(double)_offset(0), (double)_offset(1), (double)_offset(2));
|
||||||
(double)_temperature);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_thermal_offset.norm() > 0.f) {
|
if (_thermal_offset.norm() > 0.f) {
|
||||||
|
|||||||
@@ -63,7 +63,6 @@ public:
|
|||||||
void set_device_id(uint32_t device_id);
|
void set_device_id(uint32_t device_id);
|
||||||
bool set_offset(const matrix::Vector3f &offset);
|
bool set_offset(const matrix::Vector3f &offset);
|
||||||
void set_rotation(Rotation rotation);
|
void set_rotation(Rotation rotation);
|
||||||
void set_temperature(float temperature) { _temperature = temperature; };
|
|
||||||
|
|
||||||
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
|
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
|
||||||
uint8_t calibration_count() const { return _calibration_count; }
|
uint8_t calibration_count() const { return _calibration_count; }
|
||||||
@@ -104,8 +103,6 @@ public:
|
|||||||
void SensorCorrectionsUpdate(bool force = false);
|
void SensorCorrectionsUpdate(bool force = false);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static constexpr float TEMPERATURE_INVALID = -1000.f;
|
|
||||||
|
|
||||||
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
|
uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
|
||||||
|
|
||||||
Rotation _rotation_enum{ROTATION_NONE};
|
Rotation _rotation_enum{ROTATION_NONE};
|
||||||
@@ -113,7 +110,6 @@ private:
|
|||||||
matrix::Dcmf _rotation;
|
matrix::Dcmf _rotation;
|
||||||
matrix::Vector3f _offset;
|
matrix::Vector3f _offset;
|
||||||
matrix::Vector3f _thermal_offset;
|
matrix::Vector3f _thermal_offset;
|
||||||
float _temperature{NAN};
|
|
||||||
|
|
||||||
int8_t _calibration_index{-1};
|
int8_t _calibration_index{-1};
|
||||||
uint32_t _device_id{0};
|
uint32_t _device_id{0};
|
||||||
|
|||||||
@@ -192,16 +192,6 @@ bool Magnetometer::ParametersLoad()
|
|||||||
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
||||||
}
|
}
|
||||||
|
|
||||||
// CAL_MAGx_TEMP
|
|
||||||
float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
|
|
||||||
|
|
||||||
if (cal_temp > TEMPERATURE_INVALID) {
|
|
||||||
set_temperature(cal_temp);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
set_temperature(NAN);
|
|
||||||
}
|
|
||||||
|
|
||||||
// CAL_MAGx_OFF{X,Y,Z}
|
// CAL_MAGx_OFF{X,Y,Z}
|
||||||
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
|
set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
|
||||||
|
|
||||||
@@ -236,8 +226,6 @@ void Magnetometer::Reset()
|
|||||||
_power_compensation.zero();
|
_power_compensation.zero();
|
||||||
_power = 0.f;
|
_power = 0.f;
|
||||||
|
|
||||||
_temperature = NAN;
|
|
||||||
|
|
||||||
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
|
||||||
|
|
||||||
_calibration_index = -1;
|
_calibration_index = -1;
|
||||||
@@ -285,13 +273,6 @@ bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
|
|||||||
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
|
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
|
||||||
}
|
}
|
||||||
|
|
||||||
if (PX4_ISFINITE(_temperature)) {
|
|
||||||
success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
|
|
||||||
}
|
|
||||||
|
|
||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -302,20 +283,18 @@ void Magnetometer::PrintStatus()
|
|||||||
{
|
{
|
||||||
if (external()) {
|
if (external()) {
|
||||||
PX4_INFO_RAW("%s %" PRIu32
|
PX4_INFO_RAW("%s %" PRIu32
|
||||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d\n",
|
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Ext ROT: %d\n",
|
||||||
SensorString(), device_id(), enabled(),
|
SensorString(), device_id(), enabled(),
|
||||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
(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,
|
|
||||||
rotation_enum());
|
rotation_enum());
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_INFO_RAW("%s %" PRIu32
|
PX4_INFO_RAW("%s %" PRIu32
|
||||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Internal\n",
|
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], Internal\n",
|
||||||
SensorString(), device_id(), enabled(),
|
SensorString(), device_id(), enabled(),
|
||||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
(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)
|
#if defined(DEBUG_BUILD)
|
||||||
|
|||||||
@@ -66,7 +66,6 @@ public:
|
|||||||
bool set_scale(const matrix::Vector3f &scale);
|
bool set_scale(const matrix::Vector3f &scale);
|
||||||
bool set_offdiagonal(const matrix::Vector3f &offdiagonal);
|
bool set_offdiagonal(const matrix::Vector3f &offdiagonal);
|
||||||
void set_rotation(Rotation rotation);
|
void set_rotation(Rotation rotation);
|
||||||
void set_temperature(float temperature) { _temperature = temperature; };
|
|
||||||
|
|
||||||
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
|
bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
|
||||||
uint8_t calibration_count() const { return _calibration_count; }
|
uint8_t calibration_count() const { return _calibration_count; }
|
||||||
@@ -102,8 +101,6 @@ public:
|
|||||||
void UpdatePower(float power) { _power = power; }
|
void UpdatePower(float power) { _power = power; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static constexpr float TEMPERATURE_INVALID = -1000.f;
|
|
||||||
|
|
||||||
Rotation _rotation_enum{ROTATION_NONE};
|
Rotation _rotation_enum{ROTATION_NONE};
|
||||||
|
|
||||||
matrix::Dcmf _rotation;
|
matrix::Dcmf _rotation;
|
||||||
@@ -111,7 +108,6 @@ private:
|
|||||||
matrix::Matrix3f _scale;
|
matrix::Matrix3f _scale;
|
||||||
matrix::Vector3f _power_compensation;
|
matrix::Vector3f _power_compensation;
|
||||||
float _power{0.f};
|
float _power{0.f};
|
||||||
float _temperature{NAN};
|
|
||||||
|
|
||||||
int8_t _calibration_index{-1};
|
int8_t _calibration_index{-1};
|
||||||
uint32_t _device_id{0};
|
uint32_t _device_id{0};
|
||||||
|
|||||||
@@ -149,21 +149,6 @@ int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type,
|
|||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
float GetCalibrationParamFloat(const char *sensor_type, const char *cal_type, uint8_t instance)
|
|
||||||
{
|
|
||||||
// eg CAL_MAGn_TEMP
|
|
||||||
char str[20] {};
|
|
||||||
sprintf(str, "CAL_%s%" PRIu8 "_%s", sensor_type, instance, cal_type);
|
|
||||||
|
|
||||||
float value = NAN;
|
|
||||||
|
|
||||||
if (param_get(param_find(str), &value) != 0) {
|
|
||||||
PX4_ERR("failed to get %s", str);
|
|
||||||
}
|
|
||||||
|
|
||||||
return value;
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance)
|
Vector3f GetCalibrationParamsVector3f(const char *sensor_type, const char *cal_type, uint8_t instance)
|
||||||
{
|
{
|
||||||
Vector3f values{0.f, 0.f, 0.f};
|
Vector3f values{0.f, 0.f, 0.f};
|
||||||
|
|||||||
@@ -71,7 +71,6 @@ int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id
|
|||||||
* @return int32_t The calibration value.
|
* @return int32_t The calibration value.
|
||||||
*/
|
*/
|
||||||
int32_t GetCalibrationParamInt32(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.
|
* @brief Set a single calibration paramter.
|
||||||
|
|||||||
@@ -157,15 +157,13 @@ struct accel_worker_data_s {
|
|||||||
orb_advert_t *mavlink_log_pub{nullptr};
|
orb_advert_t *mavlink_log_pub{nullptr};
|
||||||
unsigned done_count{0};
|
unsigned done_count{0};
|
||||||
float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
|
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.
|
// 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],
|
static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
|
||||||
float (&accel_temperature_avg)[MAX_ACCEL_SENS], unsigned orient, unsigned samples_num)
|
unsigned orient, unsigned samples_num)
|
||||||
{
|
{
|
||||||
Vector3f accel_sum[MAX_ACCEL_SENS] {};
|
Vector3f accel_sum[MAX_ACCEL_SENS] {};
|
||||||
float temperature_sum[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
|
|
||||||
unsigned counts[MAX_ACCEL_SENS] {};
|
unsigned counts[MAX_ACCEL_SENS] {};
|
||||||
|
|
||||||
unsigned errcount = 0;
|
unsigned errcount = 0;
|
||||||
@@ -217,14 +215,6 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
|
|||||||
accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
|
accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
|
||||||
|
|
||||||
counts[accel_index]++;
|
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;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -248,8 +238,6 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
|
|||||||
for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
|
for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
|
||||||
const Vector3f avg{accel_sum[s] / counts[s]};
|
const Vector3f avg{accel_sum[s] / counts[s]};
|
||||||
avg.copyTo(accel_avg[s][orient]);
|
avg.copyTo(accel_avg[s][orient]);
|
||||||
|
|
||||||
accel_temperature_avg[s] = temperature_sum[s] / counts[s];
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return calibrate_return_ok;
|
return calibrate_return_ok;
|
||||||
@@ -263,7 +251,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",
|
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
|
||||||
detect_orientation_str(orientation));
|
detect_orientation_str(orientation));
|
||||||
|
|
||||||
read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref, orientation, samples_num);
|
read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
|
||||||
|
|
||||||
// check accel
|
// check accel
|
||||||
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
|
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
|
||||||
@@ -414,8 +402,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||||||
const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
|
const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
|
||||||
calibrations[i].set_scale(accel_T_rotated.diag());
|
calibrations[i].set_scale(accel_T_rotated.diag());
|
||||||
|
|
||||||
calibrations[i].set_temperature(worker_data.accel_temperature_ref[i]);
|
|
||||||
|
|
||||||
#if defined(DEBUD_BUILD)
|
#if defined(DEBUD_BUILD)
|
||||||
PX4_INFO("accel %d: offset", i);
|
PX4_INFO("accel %d: offset", i);
|
||||||
offset.print();
|
offset.print();
|
||||||
@@ -490,7 +476,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
|
|||||||
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
|
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
|
||||||
sensor_accel_s arp{};
|
sensor_accel_s arp{};
|
||||||
Vector3f accel_sum{};
|
Vector3f accel_sum{};
|
||||||
float temperature_sum{NAN};
|
|
||||||
unsigned count = 0;
|
unsigned count = 0;
|
||||||
|
|
||||||
while (accel_subs[accel_index].update(&arp)) {
|
while (accel_subs[accel_index].update(&arp)) {
|
||||||
@@ -526,21 +511,11 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
|
|||||||
|
|
||||||
if (diff.norm() < 1.f) {
|
if (diff.norm() < 1.f) {
|
||||||
accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
|
accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
|
||||||
|
|
||||||
count++;
|
count++;
|
||||||
|
|
||||||
if (!PX4_ISFINITE(temperature_sum)) {
|
|
||||||
// set first valid value
|
|
||||||
temperature_sum = (arp.temperature * count);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
temperature_sum += arp.temperature;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
accel_sum = accel;
|
accel_sum = accel;
|
||||||
temperature_sum = arp.temperature;
|
|
||||||
count = 1;
|
count = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -550,7 +525,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
|
|||||||
|
|
||||||
bool calibrated = false;
|
bool calibrated = false;
|
||||||
const Vector3f accel_avg = accel_sum / count;
|
const Vector3f accel_avg = accel_sum / count;
|
||||||
const float temperature_avg = temperature_sum / count;
|
|
||||||
|
|
||||||
Vector3f offset{0.f, 0.f, 0.f};
|
Vector3f offset{0.f, 0.f, 0.f};
|
||||||
|
|
||||||
@@ -593,7 +567,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
|
|||||||
|
|
||||||
} else {
|
} else {
|
||||||
calibration.set_offset(offset);
|
calibration.set_offset(offset);
|
||||||
calibration.set_temperature(temperature_avg);
|
|
||||||
|
|
||||||
if (calibration.ParametersSave(accel_index)) {
|
if (calibration.ParametersSave(accel_index)) {
|
||||||
calibration.PrintStatus();
|
calibration.PrintStatus();
|
||||||
|
|||||||
@@ -71,7 +71,6 @@ struct gyro_worker_data_t {
|
|||||||
calibration::Gyroscope calibrations[MAX_GYROS] {};
|
calibration::Gyroscope calibrations[MAX_GYROS] {};
|
||||||
|
|
||||||
Vector3f offset[MAX_GYROS] {};
|
Vector3f offset[MAX_GYROS] {};
|
||||||
float temperature[MAX_GYROS] {NAN, NAN, NAN, NAN};
|
|
||||||
|
|
||||||
math::MedianFilter<float, 9> filter[3] {};
|
math::MedianFilter<float, 9> filter[3] {};
|
||||||
};
|
};
|
||||||
@@ -119,14 +118,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
|
|||||||
|
|
||||||
calibration_counter[gyro_index]++;
|
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) {
|
if (gyro_index == 0) {
|
||||||
worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
|
worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
|
||||||
worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
|
worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
|
||||||
@@ -169,7 +160,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
|
|||||||
}
|
}
|
||||||
|
|
||||||
worker_data.offset[s] /= calibration_counter[s];
|
worker_data.offset[s] /= calibration_counter[s];
|
||||||
worker_data.temperature[s] /= calibration_counter[s];
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return calibrate_return_ok;
|
return calibrate_return_ok;
|
||||||
@@ -269,8 +259,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
|||||||
|
|
||||||
if (calibration.device_id() != 0) {
|
if (calibration.device_id() != 0) {
|
||||||
calibration.set_offset(worker_data.offset[uorb_index]);
|
calibration.set_offset(worker_data.offset[uorb_index]);
|
||||||
calibration.set_temperature(worker_data.temperature[uorb_index]);
|
|
||||||
|
|
||||||
calibration.PrintStatus();
|
calibration.PrintStatus();
|
||||||
|
|
||||||
if (calibration.ParametersSave(uorb_index, true)) {
|
if (calibration.ParametersSave(uorb_index, true)) {
|
||||||
|
|||||||
@@ -94,8 +94,6 @@ struct mag_worker_data_t {
|
|||||||
float *y[MAX_MAGS];
|
float *y[MAX_MAGS];
|
||||||
float *z[MAX_MAGS];
|
float *z[MAX_MAGS];
|
||||||
|
|
||||||
float temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
|
|
||||||
|
|
||||||
calibration::Magnetometer calibration[MAX_MAGS] {};
|
calibration::Magnetometer calibration[MAX_MAGS] {};
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -342,7 +340,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||||||
if (mag_sub[0].updatedBlocking(1000_ms)) {
|
if (mag_sub[0].updatedBlocking(1000_ms)) {
|
||||||
bool rejected = false;
|
bool rejected = false;
|
||||||
Vector3f new_samples[MAX_MAGS] {};
|
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++) {
|
for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
|
||||||
if (worker_data->calibration[cur_mag].device_id() != 0) {
|
if (worker_data->calibration[cur_mag].device_id() != 0) {
|
||||||
@@ -371,7 +368,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||||||
|
|
||||||
if (!reject) {
|
if (!reject) {
|
||||||
new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
|
new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
|
||||||
new_temperature[cur_mag] = mag.temperature;
|
|
||||||
updated = true;
|
updated = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -392,14 +388,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
|||||||
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1);
|
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);
|
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]++;
|
worker_data->calibration_counter_total[cur_mag]++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -912,8 +900,6 @@ 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_offdiagonal(offdiag[cur_mag]);
|
||||||
}
|
}
|
||||||
|
|
||||||
current_cal.set_temperature(worker_data.temperature[cur_mag]);
|
|
||||||
|
|
||||||
current_cal.PrintStatus();
|
current_cal.PrintStatus();
|
||||||
|
|
||||||
if (current_cal.ParametersSave(cur_mag, true)) {
|
if (current_cal.ParametersSave(cur_mag, true)) {
|
||||||
@@ -1019,7 +1005,6 @@ 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
|
// 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);
|
const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);
|
||||||
cal.set_offset(offset);
|
cal.set_offset(offset);
|
||||||
cal.set_temperature(mag.temperature);
|
|
||||||
|
|
||||||
// save new calibration
|
// save new calibration
|
||||||
if (cal.ParametersSave(cur_mag)) {
|
if (cal.ParametersSave(cur_mag)) {
|
||||||
|
|||||||
@@ -246,7 +246,6 @@ void GyroCalibration::Run()
|
|||||||
const Vector3f old_offset{_gyro_calibration[gyro].offset()};
|
const Vector3f old_offset{_gyro_calibration[gyro].offset()};
|
||||||
|
|
||||||
if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) {
|
if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) {
|
||||||
_gyro_calibration[gyro].set_temperature(_temperature[gyro]);
|
|
||||||
|
|
||||||
calibration_updated = true;
|
calibration_updated = true;
|
||||||
|
|
||||||
|
|||||||
@@ -159,18 +159,6 @@ parameters:
|
|||||||
num_instances: *max_num_sensor_instances
|
num_instances: *max_num_sensor_instances
|
||||||
instance_start: 0
|
instance_start: 0
|
||||||
|
|
||||||
CAL_ACC${i}_TEMP:
|
|
||||||
description:
|
|
||||||
short: Accelerometer ${i} calibration temperature
|
|
||||||
long: Temperature during last calibration.
|
|
||||||
category: System
|
|
||||||
type: float
|
|
||||||
default: -1000.
|
|
||||||
unit: celcius
|
|
||||||
volatile: true
|
|
||||||
num_instances: *max_num_sensor_instances
|
|
||||||
instance_start: 0
|
|
||||||
|
|
||||||
# Gyroscope calibration
|
# Gyroscope calibration
|
||||||
CAL_GYRO${i}_ID:
|
CAL_GYRO${i}_ID:
|
||||||
description:
|
description:
|
||||||
@@ -288,18 +276,6 @@ parameters:
|
|||||||
num_instances: *max_num_sensor_instances
|
num_instances: *max_num_sensor_instances
|
||||||
instance_start: 0
|
instance_start: 0
|
||||||
|
|
||||||
CAL_GYRO${i}_TEMP:
|
|
||||||
description:
|
|
||||||
short: Gyroscope ${i} calibration temperature
|
|
||||||
long: Temperature during last calibration.
|
|
||||||
category: System
|
|
||||||
type: float
|
|
||||||
default: -1000.
|
|
||||||
unit: celcius
|
|
||||||
volatile: true
|
|
||||||
num_instances: *max_num_sensor_instances
|
|
||||||
instance_start: 0
|
|
||||||
|
|
||||||
# Magnetometer calibration
|
# Magnetometer calibration
|
||||||
CAL_MAG${i}_ID:
|
CAL_MAG${i}_ID:
|
||||||
description:
|
description:
|
||||||
@@ -530,15 +506,3 @@ parameters:
|
|||||||
volatile: true
|
volatile: true
|
||||||
num_instances: *max_num_sensor_instances
|
num_instances: *max_num_sensor_instances
|
||||||
instance_start: 0
|
instance_start: 0
|
||||||
|
|
||||||
CAL_MAG${i}_TEMP:
|
|
||||||
description:
|
|
||||||
short: Magnetometer ${i} calibration temperature
|
|
||||||
long: Temperature during last calibration.
|
|
||||||
category: System
|
|
||||||
type: float
|
|
||||||
default: -1000.
|
|
||||||
unit: celcius
|
|
||||||
volatile: true
|
|
||||||
num_instances: *max_num_sensor_instances
|
|
||||||
instance_start: 0
|
|
||||||
|
|||||||
@@ -194,7 +194,6 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
|
|||||||
const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
|
const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
|
||||||
|
|
||||||
if (_calibration[mag_index].set_offset(offset)) {
|
if (_calibration[mag_index].set_offset(offset)) {
|
||||||
_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
|
|
||||||
|
|
||||||
// save parameters with preferred calibration slot to current sensor index
|
// save parameters with preferred calibration slot to current sensor index
|
||||||
_calibration[mag_index].ParametersSave(mag_index);
|
_calibration[mag_index].ParametersSave(mag_index);
|
||||||
@@ -253,7 +252,6 @@ void VehicleMagnetometer::UpdateMagCalibration()
|
|||||||
_calibration_estimator_bias[mag_index];
|
_calibration_estimator_bias[mag_index];
|
||||||
|
|
||||||
_mag_cal[i].variance = bias_variance;
|
_mag_cal[i].variance = bias_variance;
|
||||||
_mag_cal[i].temperature = _last_data[mag_index].temperature;
|
|
||||||
|
|
||||||
_in_flight_mag_cal_available = true;
|
_in_flight_mag_cal_available = true;
|
||||||
break;
|
break;
|
||||||
@@ -298,8 +296,6 @@ void VehicleMagnetometer::UpdateMagCalibration()
|
|||||||
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
|
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
|
||||||
(double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2));
|
(double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2));
|
||||||
|
|
||||||
_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
|
|
||||||
|
|
||||||
_calibration[mag_index].ParametersSave();
|
_calibration[mag_index].ParametersSave();
|
||||||
|
|
||||||
calibration_param_save_needed = true;
|
calibration_param_save_needed = true;
|
||||||
|
|||||||
@@ -119,7 +119,6 @@ private:
|
|||||||
uint32_t device_id{0};
|
uint32_t device_id{0};
|
||||||
matrix::Vector3f offset{};
|
matrix::Vector3f offset{};
|
||||||
matrix::Vector3f variance{};
|
matrix::Vector3f variance{};
|
||||||
float temperature{NAN};
|
|
||||||
} _mag_cal[ORB_MULTI_MAX_INSTANCES] {};
|
} _mag_cal[ORB_MULTI_MAX_INSTANCES] {};
|
||||||
|
|
||||||
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
|
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
|
||||||
|
|||||||
Reference in New Issue
Block a user