fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 19:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

move initial sensor priority to parameters and purge ORB_PRIORITY

Browse Source 
- CAL_ACCx_EN -> CAL_ACCx_PRIO
 - CAL_GYROx_EN -> CAL_GYROx_PRIO
 - CAL_MAGx_EN -> CAL_MAGx_PRIO
This commit is contained in:
Daniel Agar
2020-08-21 10:12:13 -04:00
parent 971b1e4b4d
commit 27f23ac290
124 changed files with 463 additions and 509 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Accelerometer.cpp
+18 -5
View File
@@ -118,9 +118,15 @@ void Accelerometer::ParametersUpdate()
_rotation.setIdentity();
}
// CAL_ACCx_EN
int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
_enabled = (enabled == 1);
// CAL_ACCx_PRIO
_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);
if (_priority < 0 || _priority > 100) {
// reset to default
PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, DEFAULT_PRIORITY);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, DEFAULT_PRIORITY);
_priority = DEFAULT_PRIORITY;
}
// CAL_ACCx_OFF{X,Y,Z}
_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
@@ -140,7 +146,7 @@ void Accelerometer::Reset()
_scale = Vector3f{1.f, 1.f, 1.f};
_thermal_offset.zero();
_enabled = true;
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
}
@@ -150,10 +156,17 @@ bool Accelerometer::ParametersSave()
if (_calibration_index >= 0) {
// save calibration
SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
SetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index, _scale);
// if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
// } else {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// }
return true;
}
src/lib/sensor_calibration/Accelerometer.hpp
+6 -2
View File
@@ -47,6 +47,9 @@ class Accelerometer
public:
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "ACC"; }
Accelerometer();
@@ -63,8 +66,9 @@ public:
void set_scale(const matrix::Vector3f &scale) { _scale = scale; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return _enabled; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
// apply offsets and scale
@@ -91,8 +95,8 @@ private:
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{DEFAULT_PRIORITY};
bool _enabled{true};
bool _external{false};
};
} // namespace calibration
src/lib/sensor_calibration/Gyroscope.cpp
+18 -5
View File
@@ -118,9 +118,15 @@ void Gyroscope::ParametersUpdate()
_rotation.setIdentity();
}
// CAL_GYROx_EN
int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
_enabled = (enabled == 1);
// CAL_GYROx_PRIO
_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);
if (_priority < 0 || _priority > 100) {
// reset to default
PX4_ERR("%s %d invalid priority %d, resetting to %d", SensorString(), _calibration_index, _priority, DEFAULT_PRIORITY);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, DEFAULT_PRIORITY);
_priority = DEFAULT_PRIORITY;
}
// CAL_GYROx_OFF{X,Y,Z}
_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
@@ -136,7 +142,7 @@ void Gyroscope::Reset()
_offset.zero();
_thermal_offset.zero();
_enabled = true;
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
}
@@ -146,9 +152,16 @@ bool Gyroscope::ParametersSave()
if (_calibration_index >= 0) {
// save calibration
SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
// if (_external) {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
// } else {
// SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
// }
return true;
}
src/lib/sensor_calibration/Gyroscope.hpp
+6 -2
View File
@@ -47,6 +47,9 @@ class Gyroscope
public:
static constexpr int MAX_SENSOR_COUNT = 3;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "GYRO"; }
Gyroscope();
@@ -62,8 +65,9 @@ public:
void set_offset(const matrix::Vector3f &offset) { _offset = offset; }
uint32_t device_id() const { return _device_id; }
bool enabled() const { return _enabled; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
// apply offsets and scale
@@ -89,8 +93,8 @@ private:
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{DEFAULT_PRIORITY};
bool _enabled{true};
bool _external{false};
};
} // namespace calibration
src/lib/sensor_calibration/Magnetometer.cpp
+27 -5
View File
@@ -64,6 +64,21 @@ void Magnetometer::set_device_id(uint32_t device_id)
void Magnetometer::set_external(bool external)
{
// update priority default appropriately if not set
if (_calibration_index < 0) {
if ((_priority < 0) || (_priority > 100)) {
_priority = external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
} else if (!_external && external && (_priority == DEFAULT_PRIORITY)) {
// internal -> external
_priority = DEFAULT_EXTERNAL_PRIORITY;
} else if (_external && !external && (_priority == DEFAULT_EXTERNAL_PRIORITY)) {
// external -> internal
_priority = DEFAULT_PRIORITY;
}
}
_external = external;
}
@@ -113,9 +128,16 @@ void Magnetometer::ParametersUpdate()
_rotation = get_rot_matrix((enum Rotation)rotation);
}
// CAL_MAGx_EN
int32_t enabled = GetCalibrationParam(SensorString(), "EN", _calibration_index);
_enabled = (enabled == 1);
// CAL_MAGx_PRIO
_priority = GetCalibrationParam(SensorString(), "PRIO", _calibration_index);
if ((_priority < 0) || (_priority > 100)) {
// reset to default
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);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, new_priority);
_priority = new_priority;
}
// CAL_MAGx_OFF{X,Y,Z}
_offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index);
@@ -151,7 +173,7 @@ void Magnetometer::Reset()
_power_compensation.zero();
_power = 0.f;
_enabled = true;
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
_calibration_index = -1;
}
@@ -161,7 +183,7 @@ bool Magnetometer::ParametersSave()
if (_calibration_index >= 0) {
// save calibration
SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);
SetCalibrationParam(SensorString(), "EN", _calibration_index, _enabled ? 1 : 0);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, _priority);
SetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index, _offset);
Vector3f scale{_scale.diag()};
src/lib/sensor_calibration/Magnetometer.hpp
+6 -2
View File
@@ -48,6 +48,9 @@ class Magnetometer
public:
static constexpr int MAX_SENSOR_COUNT = 4;
static constexpr uint8_t DEFAULT_PRIORITY = 50;
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
static constexpr const char *SensorString() { return "MAG"; }
Magnetometer();
@@ -66,9 +69,10 @@ public:
void set_rotation(Rotation rotation);
uint32_t device_id() const { return _device_id; }
bool enabled() const { return _enabled; }
bool enabled() const { return (_priority > 0); }
bool external() const { return _external; }
const matrix::Vector3f &offset() const { return _offset; }
const int32_t &priority() const { return _priority; }
const matrix::Dcmf &rotation() const { return _rotation; }
const Rotation &rotation_enum() const { return _rotation_enum; }
const matrix::Matrix3f &scale() const { return _scale; }
@@ -98,8 +102,8 @@ private:
int8_t _calibration_index{-1};
uint32_t _device_id{0};
int32_t _priority{-1};
bool _enabled{true};
bool _external{false};
};
} // namespace calibration