Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

lib/sensor_calibration: Magnetometer catch and fix invalid rotations

Browse Source 
- improve status print
This commit is contained in:
Daniel Agar
2020-08-20 11:32:17 -04:00
parent c76cdaf8a8
commit ea13bf271d
2 changed files with 41 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Magnetometer.cpp
+37 -9
View File
@@ -48,8 +48,9 @@ Magnetometer::Magnetometer()
Reset(); Reset();
} }
Magnetometer::Magnetometer(uint32_t device_id) Magnetometer::Magnetometer(uint32_t device_id, bool external)
{ {
set_external(external);
Reset(); Reset();
set_device_id(device_id); set_device_id(device_id);
} }
@@ -118,14 +119,30 @@ void Magnetometer::ParametersUpdate()
if (_calibration_index >= 0) { if (_calibration_index >= 0) {
if (!_external) { // CAL_MAGx_ROT
_rotation = GetBoardRotation(); int32_t rotation_value = GetCalibrationParam(SensorString(), "ROT", _calibration_index);
_rotation_enum = ROTATION_NONE;
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
PX4_ERR("External %s %d (%d) invalid rotation %d, resetting to rotation none",
SensorString(), _device_id, _calibration_index, rotation_value);
rotation_value = ROTATION_NONE;
SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
}
_rotation_enum = static_cast<Rotation>(rotation_value);
_rotation = get_rot_matrix(_rotation_enum);
} else { } else {
int32_t rotation = GetCalibrationParam(SensorString(), "ROT", _calibration_index); // internal mag, CAL_MAGx_ROT -1
_rotation_enum = static_cast<Rotation>(rotation); if (rotation_value != -1) {
_rotation = get_rot_matrix((enum Rotation)rotation); PX4_ERR("Internal %s %d (%d) invalid rotation %d, resetting",
SensorString(), _device_id, _calibration_index, rotation_value);
SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
_rotation = GetBoardRotation();
_rotation_enum = ROTATION_NONE;
} }
// CAL_MAGx_PRIO // CAL_MAGx_PRIO
@@ -134,7 +151,8 @@ void Magnetometer::ParametersUpdate()
if ((_priority < 0) || (_priority > 100)) { if ((_priority < 0) || (_priority > 100)) {
// reset to default // reset to default
int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY; int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, new_priority); PX4_ERR("%s %d (%d) invalid priority %d, resetting to %d",
SensorString(), _device_id, _calibration_index, _priority, new_priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, new_priority); SetCalibrationParam(SensorString(), "PRIO", _calibration_index, new_priority);
_priority = new_priority; _priority = new_priority;
} }
@@ -209,9 +227,19 @@ bool Magnetometer::ParametersSave()
void Magnetometer::PrintStatus() void Magnetometer::PrintStatus()
{ {
PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), device_id(), enabled(), if (external()) {
PX4_INFO("%s %d EN: %d, offset: [% 05.3f % 05.3f % 05.3f], scale: [% 05.3f % 05.3f % 05.3f], 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),
rotation_enum());
} else {
PX4_INFO("%s %d EN: %d, offset: [% 05.3f % 05.3f % 05.3f], scale: [% 05.3f % 05.3f % 05.3f], Internal",
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));
}
#if defined(DEBUG_BUILD) #if defined(DEBUG_BUILD)
_scale.print() _scale.print()
src/lib/sensor_calibration/Magnetometer.hpp
+2 -2
View File
@@ -54,7 +54,7 @@ public:
static constexpr const char *SensorString() { return "MAG"; } static constexpr const char *SensorString() { return "MAG"; }
Magnetometer(); Magnetometer();
explicit Magnetometer(uint32_t device_id); explicit Magnetometer(uint32_t device_id, bool external = false);
~Magnetometer() = default; ~Magnetometer() = default;
@@ -92,7 +92,7 @@ public:
void UpdatePower(float power) { _power = power; } void UpdatePower(float power) { _power = power; }
private: private:
Rotation _rotation_enum; Rotation _rotation_enum{ROTATION_NONE};
matrix::Dcmf _rotation; matrix::Dcmf _rotation;
matrix::Vector3f _offset; matrix::Vector3f _offset;