fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 18:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors (accel/gyro/mag) determine if external with device id

Browse Source
This commit is contained in:
Daniel Agar
2022-01-10 10:31:07 -05:00
committed by GitHub
parent 45040be669
commit e731fcdbc0
36 changed files with 187 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/sensor_calibration/Gyroscope.cpp
+36 -47
View File
@@ -48,43 +48,27 @@ Gyroscope::Gyroscope()
Reset();
}
Gyroscope::Gyroscope(uint32_t device_id, bool external)
Gyroscope::Gyroscope(uint32_t device_id)
{
Reset();
set_device_id(device_id, external);
set_device_id(device_id);
}
void Gyroscope::set_device_id(uint32_t device_id, bool external)
void Gyroscope::set_device_id(uint32_t device_id)
{
bool external = DeviceExternal(device_id);
if (_device_id != device_id || _external != external) {
set_external(external);
_device_id = device_id;
_external = external;
Reset();
ParametersUpdate();
SensorCorrectionsUpdate(true);
}
}
void Gyroscope::set_external(bool external)
{
// update priority default appropriately if not set
if (_calibration_index < 0 || _priority < 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;
}
void Gyroscope::SensorCorrectionsUpdate(bool force)
{
// check if the selected sensor has updated
@@ -147,7 +131,11 @@ void Gyroscope::set_rotation(Rotation rotation)
void Gyroscope::ParametersUpdate()
{
if (_device_id != 0) {
if (_device_id == 0) {
return;
}
if (_calibration_index < 0) {
_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
}
@@ -158,22 +146,13 @@ void Gyroscope::ParametersUpdate()
if (_external) {
if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
PX4_WARN("External %s %" PRIu32 " (%" PRId8 ") invalid rotation %" PRId32 ", resetting to rotation none",
SensorString(), _device_id, _calibration_index, rotation_value);
// invalid rotation, resetting
rotation_value = ROTATION_NONE;
SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
}
set_rotation(static_cast<Rotation>(rotation_value));
} else {
// internal, CAL_GYROx_ROT -1
if (rotation_value != -1) {
PX4_ERR("Internal %s %" PRIu32 " (%" PRId8 ") invalid rotation %" PRId32 " resetting",
SensorString(), _device_id, _calibration_index, rotation_value);
SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
}
// internal sensors follow board rotation
set_rotation(GetBoardRotation());
}
@@ -183,16 +162,16 @@ void Gyroscope::ParametersUpdate()
if ((_priority < 0) || (_priority > 100)) {
// reset to default, -1 is the uninitialized parameter value
int32_t new_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
static constexpr int32_t CAL_PRIO_UNINITIALIZED = -1;
if (_priority != -1) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting to %" PRId32, SensorString(), _device_id,
_calibration_index, _priority, new_priority);
if (_priority != CAL_PRIO_UNINITIALIZED) {
PX4_ERR("%s %" PRIu32 " (%" PRId8 ") invalid priority %" PRId32 ", resetting", SensorString(), _device_id,
_calibration_index, _priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, new_priority);
SetCalibrationParam(SensorString(), "PRIO", _calibration_index, CAL_PRIO_UNINITIALIZED);
}
_priority = new_priority;
_priority = _external ? DEFAULT_EXTERNAL_PRIORITY : DEFAULT_PRIORITY;
}
// CAL_GYROx_TEMP
@@ -248,7 +227,7 @@ bool Gyroscope::ParametersSave()
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, (int32_t)_rotation_enum);
} else {
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
}
if (PX4_ISFINITE(_temperature)) {
@@ -266,10 +245,20 @@ bool Gyroscope::ParametersSave()
void Gyroscope::PrintStatus()
{
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 (external()) {
PX4_INFO("%s %" PRIu32
" EN: %d, offset: [%05.3f %05.3f %05.3f], %.1f degC, Ext ROT: %d",
SensorString(), device_id(), enabled(),
(double)_offset(0), (double)_offset(1), (double)_offset(2),
(double)_temperature,
rotation_enum());
} else {
PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], %.1f degC, Internal",
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,