Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 13:15:32 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

vehicle_imu/vehicle_magnetometer add calibration indicator to message

Browse Source 
- vehicle_imu/vehicle_magnetometer add monotonically increasing `calibration_count` field so that downstream subscribers are aware of calibration changes
This commit is contained in:
Daniel Agar
2020-09-15 13:12:57 -04:00
committed by GitHub
parent 87471a988d
commit c41f053c7b
10 changed files with 79 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Accelerometer.cpp
+20 -2
View File
@@ -149,11 +149,27 @@ void Accelerometer::ParametersUpdate()
_priority = DEFAULT_PRIORITY;
}
bool calibration_changed = false;
// CAL_ACCx_OFF{X,Y,Z}
_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_offset = offset;
}
// CAL_ACCx_SCALE{X,Y,Z}
_scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
const Vector3f scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
if (Vector3f(_scale - scale).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_scale = scale;
}
if (calibration_changed) {
_calibration_count++;
}
} else {
Reset();
@@ -170,6 +186,8 @@ void Accelerometer::Reset()
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
}
bool Accelerometer::ParametersSave()
src/lib/sensor_calibration/Accelerometer.hpp
+3
View File
@@ -65,6 +65,7 @@ public:
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
uint8_t calibration_count() const { return _calibration_count; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
@@ -98,5 +99,7 @@ private:
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration
src/lib/sensor_calibration/Gyroscope.cpp
+14 -1
View File
@@ -149,8 +149,19 @@ void Gyroscope::ParametersUpdate()
_priority = DEFAULT_PRIORITY;
}
bool calibration_changed = false;
// CAL_GYROx_OFF{X,Y,Z}
_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_offset = offset;
}
if (calibration_changed) {
_calibration_count++;
}
} else {
Reset();
@@ -166,6 +177,8 @@ void Gyroscope::Reset()
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
}
bool Gyroscope::ParametersSave()
src/lib/sensor_calibration/Gyroscope.hpp
+3
View File
@@ -64,6 +64,7 @@ public:
void set_external(bool external = true);
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
uint8_t calibration_count() const { return _calibration_count; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
@@ -96,5 +97,7 @@ private:
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration
src/lib/sensor_calibration/Magnetometer.cpp
+30 -7
View File
@@ -157,21 +157,42 @@ void Magnetometer::ParametersUpdate()
_priority = new_priority;
}
bool calibration_changed = false;
// CAL_MAGx_OFF{X,Y,Z}
_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
_offset = offset;
}
// CAL_MAGx_SCALE{X,Y,Z}
const Vector3f diag = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index);
if (Vector3f(_scale.diag() - diag).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
}
// CAL_MAGx_ODIAG{X,Y,Z}
const Vector3f offdiag = GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index);
float scale[9] {
diag(0), offdiag(0), offdiag(1),
offdiag(0), diag(1), offdiag(2),
offdiag(1), offdiag(2), diag(2)
};
_scale = Matrix3f{scale};
if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiag).norm_squared() > 0.001f * 0.001f) {
calibration_changed = true;
}
if (calibration_changed) {
float scale[9] {
diag(0), offdiag(0), offdiag(1),
offdiag(0), diag(1), offdiag(2),
offdiag(1), offdiag(2), diag(2)
};
_scale = Matrix3f{scale};
_calibration_count++;
}
// CAL_MAGx_COMP{X,Y,Z}
_power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index);
@@ -194,6 +215,8 @@ void Magnetometer::Reset()
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
_calibration_count = 0;
}
bool Magnetometer::ParametersSave()
src/lib/sensor_calibration/Magnetometer.hpp
+3
View File
@@ -68,6 +68,7 @@ public:
void set_offdiagonal(const matrix::Vector3f &offdiagonal);
void set_rotation(Rotation rotation);
uint8_t calibration_count() const { return _calibration_count; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
@@ -105,5 +106,7 @@ private:
int32_t _priority{-1};
bool _external{false};
uint8_t _calibration_count{0};
};
} // namespace calibration