mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 14:03:33 +08:00
sensors (accel/gyro/mag) determine if external with device id
This commit is contained in:
@@ -75,8 +75,6 @@ void PX4Magnetometer::update(const hrt_abstime ×tamp_sample, float x, float
|
||||
report.y = y * _scale;
|
||||
report.z = z * _scale;
|
||||
|
||||
report.is_external = _external;
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
_sensor_pub.publish(report);
|
||||
}
|
||||
|
||||
@@ -44,15 +44,12 @@ public:
|
||||
PX4Magnetometer(uint32_t device_id, enum Rotation rotation = ROTATION_NONE);
|
||||
~PX4Magnetometer();
|
||||
|
||||
bool external() { return _external; }
|
||||
|
||||
void set_device_id(uint32_t device_id) { _device_id = device_id; }
|
||||
void set_device_type(uint8_t devtype);
|
||||
void set_error_count(uint32_t error_count) { _error_count = error_count; }
|
||||
void increase_error_count() { _error_count++; }
|
||||
void set_scale(float scale) { _scale = scale; }
|
||||
void set_temperature(float temperature) { _temperature = temperature; }
|
||||
void set_external(bool external) { _external = external; }
|
||||
|
||||
void update(const hrt_abstime ×tamp_sample, float x, float y, float z);
|
||||
|
||||
@@ -67,6 +64,4 @@ private:
|
||||
float _scale{1.f};
|
||||
float _temperature{NAN};
|
||||
uint32_t _error_count{0};
|
||||
|
||||
bool _external{false};
|
||||
};
|
||||
|
||||
@@ -48,43 +48,27 @@ Accelerometer::Accelerometer()
|
||||
Reset();
|
||||
}
|
||||
|
||||
Accelerometer::Accelerometer(uint32_t device_id, bool external)
|
||||
Accelerometer::Accelerometer(uint32_t device_id)
|
||||
{
|
||||
Reset();
|
||||
set_device_id(device_id, external);
|
||||
set_device_id(device_id);
|
||||
}
|
||||
|
||||
void Accelerometer::set_device_id(uint32_t device_id, bool external)
|
||||
void Accelerometer::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 Accelerometer::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 Accelerometer::SensorCorrectionsUpdate(bool force)
|
||||
{
|
||||
// check if the selected sensor has updated
|
||||
@@ -162,7 +146,11 @@ void Accelerometer::set_rotation(Rotation rotation)
|
||||
|
||||
void Accelerometer::ParametersUpdate()
|
||||
{
|
||||
if (_device_id != 0) {
|
||||
if (_device_id == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_calibration_index < 0) {
|
||||
_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
|
||||
}
|
||||
|
||||
@@ -173,22 +161,13 @@ void Accelerometer::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_ACCx_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());
|
||||
}
|
||||
@@ -198,16 +177,16 @@ void Accelerometer::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_ACCx_TEMP
|
||||
@@ -268,7 +247,7 @@ bool Accelerometer::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)) {
|
||||
@@ -286,11 +265,22 @@ bool Accelerometer::ParametersSave()
|
||||
|
||||
void Accelerometer::PrintStatus()
|
||||
{
|
||||
PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%.4f %.4f %.4f], scale: [%.4f %.4f %.4f], %.1f degC",
|
||||
SensorString(), device_id(), enabled(),
|
||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
||||
(double)_scale(0), (double)_scale(1), (double)_scale(2),
|
||||
(double)_temperature);
|
||||
if (external()) {
|
||||
PX4_INFO("%s %" PRIu32
|
||||
" EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%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)_scale(0), (double)_scale(1), (double)_scale(2),
|
||||
(double)_temperature,
|
||||
rotation_enum());
|
||||
|
||||
} else {
|
||||
PX4_INFO("%s %" PRIu32 " EN: %d, offset: [%05.3f %05.3f %05.3f], scale: [%05.3f %05.3f %05.3f], %.1f degC, Internal",
|
||||
SensorString(), device_id(), enabled(),
|
||||
(double)_offset(0), (double)_offset(1), (double)_offset(2),
|
||||
(double)_scale(0), (double)_scale(1), (double)_scale(2),
|
||||
(double)_temperature);
|
||||
}
|
||||
|
||||
if (_thermal_offset.norm() > 0.f) {
|
||||
PX4_INFO("%s %" PRIu32 " temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id,
|
||||
|
||||
@@ -48,20 +48,19 @@ public:
|
||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||
|
||||
static constexpr uint8_t DEFAULT_PRIORITY = 50;
|
||||
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 25;
|
||||
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
|
||||
|
||||
static constexpr const char *SensorString() { return "ACC"; }
|
||||
|
||||
Accelerometer();
|
||||
explicit Accelerometer(uint32_t device_id, bool external = false);
|
||||
explicit Accelerometer(uint32_t device_id);
|
||||
|
||||
~Accelerometer() = default;
|
||||
|
||||
void PrintStatus();
|
||||
|
||||
void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
|
||||
void set_device_id(uint32_t device_id, bool external = false);
|
||||
void set_external(bool external = true);
|
||||
void set_device_id(uint32_t device_id);
|
||||
bool set_offset(const matrix::Vector3f &offset);
|
||||
bool set_scale(const matrix::Vector3f &scale);
|
||||
void set_rotation(Rotation rotation);
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -48,20 +48,19 @@ public:
|
||||
static constexpr int MAX_SENSOR_COUNT = 4;
|
||||
|
||||
static constexpr uint8_t DEFAULT_PRIORITY = 50;
|
||||
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 25;
|
||||
static constexpr uint8_t DEFAULT_EXTERNAL_PRIORITY = 75;
|
||||
|
||||
static constexpr const char *SensorString() { return "GYRO"; }
|
||||
|
||||
Gyroscope();
|
||||
explicit Gyroscope(uint32_t device_id, bool external = false);
|
||||
explicit Gyroscope(uint32_t device_id);
|
||||
|
||||
~Gyroscope() = default;
|
||||
|
||||
void PrintStatus();
|
||||
|
||||
void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
|
||||
void set_device_id(uint32_t device_id, bool external = false);
|
||||
void set_external(bool external = true);
|
||||
void set_device_id(uint32_t device_id);
|
||||
bool set_offset(const matrix::Vector3f &offset);
|
||||
void set_rotation(Rotation rotation);
|
||||
void set_temperature(float temperature) { _temperature = temperature; };
|
||||
|
||||
@@ -48,42 +48,26 @@ Magnetometer::Magnetometer()
|
||||
Reset();
|
||||
}
|
||||
|
||||
Magnetometer::Magnetometer(uint32_t device_id, bool external)
|
||||
Magnetometer::Magnetometer(uint32_t device_id)
|
||||
{
|
||||
Reset();
|
||||
set_device_id(device_id, external);
|
||||
set_device_id(device_id);
|
||||
}
|
||||
|
||||
void Magnetometer::set_device_id(uint32_t device_id, bool external)
|
||||
void Magnetometer::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();
|
||||
}
|
||||
}
|
||||
|
||||
void Magnetometer::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;
|
||||
}
|
||||
|
||||
bool Magnetometer::set_offset(const Vector3f &offset)
|
||||
{
|
||||
if (Vector3f(_offset - offset).longerThan(0.01f)) {
|
||||
@@ -147,7 +131,11 @@ void Magnetometer::set_rotation(Rotation rotation)
|
||||
|
||||
void Magnetometer::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 Magnetometer::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 mag, CAL_MAGx_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 Magnetometer::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_MAGx_TEMP
|
||||
@@ -268,7 +247,7 @@ bool Magnetometer::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)) {
|
||||
|
||||
@@ -54,15 +54,14 @@ public:
|
||||
static constexpr const char *SensorString() { return "MAG"; }
|
||||
|
||||
Magnetometer();
|
||||
explicit Magnetometer(uint32_t device_id, bool external = false);
|
||||
explicit Magnetometer(uint32_t device_id);
|
||||
|
||||
~Magnetometer() = default;
|
||||
|
||||
void PrintStatus();
|
||||
|
||||
void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
|
||||
void set_device_id(uint32_t device_id, bool external = false);
|
||||
void set_external(bool external = true);
|
||||
void set_device_id(uint32_t device_id);
|
||||
bool set_offset(const matrix::Vector3f &offset);
|
||||
bool set_scale(const matrix::Vector3f &scale);
|
||||
bool set_offdiagonal(const matrix::Vector3f &offdiagonal);
|
||||
|
||||
@@ -34,9 +34,18 @@
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/drivers/device/Device.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/parameters/param.h>
|
||||
|
||||
#if defined(CONFIG_I2C)
|
||||
# include <px4_platform_common/i2c.h>
|
||||
#endif // CONFIG_I2C
|
||||
|
||||
#if defined(CONFIG_SPI)
|
||||
# include <px4_platform_common/spi.h>
|
||||
#endif // CONFIG_SPI
|
||||
|
||||
using math::radians;
|
||||
using matrix::Eulerf;
|
||||
using matrix::Dcmf;
|
||||
@@ -179,4 +188,51 @@ Dcmf GetBoardRotationMatrix()
|
||||
return get_rot_matrix(GetBoardRotation());
|
||||
}
|
||||
|
||||
bool DeviceExternal(uint32_t device_id)
|
||||
{
|
||||
bool external = true;
|
||||
|
||||
// decode device id to determine if external
|
||||
union device::Device::DeviceId id {};
|
||||
id.devid = device_id;
|
||||
|
||||
const device::Device::DeviceBusType bus_type = id.devid_s.bus_type;
|
||||
|
||||
switch (bus_type) {
|
||||
case device::Device::DeviceBusType_I2C:
|
||||
#if defined(CONFIG_I2C)
|
||||
external = px4_i2c_bus_external(id.devid_s.bus);
|
||||
#endif // CONFIG_I2C
|
||||
break;
|
||||
|
||||
case device::Device::DeviceBusType_SPI:
|
||||
#if defined(CONFIG_SPI)
|
||||
external = px4_spi_bus_external(id.devid_s.bus);
|
||||
#endif // CONFIG_SPI
|
||||
break;
|
||||
|
||||
case device::Device::DeviceBusType_UAVCAN:
|
||||
external = true;
|
||||
break;
|
||||
|
||||
case device::Device::DeviceBusType_SIMULATION:
|
||||
external = false;
|
||||
break;
|
||||
|
||||
case device::Device::DeviceBusType_SERIAL:
|
||||
external = true;
|
||||
break;
|
||||
|
||||
case device::Device::DeviceBusType_MAVLINK:
|
||||
external = true;
|
||||
break;
|
||||
|
||||
case device::Device::DeviceBusType_UNKNOWN:
|
||||
external = true;
|
||||
break;
|
||||
}
|
||||
|
||||
return external;
|
||||
}
|
||||
|
||||
} // namespace calibration
|
||||
|
||||
@@ -132,4 +132,11 @@ Rotation GetBoardRotation();
|
||||
*/
|
||||
matrix::Dcmf GetBoardRotationMatrix();
|
||||
|
||||
/**
|
||||
* @brief Determine if device is on an external bus
|
||||
*
|
||||
* @return true if device is on an external bus
|
||||
*/
|
||||
bool DeviceExternal(uint32_t device_id);
|
||||
|
||||
} // namespace calibration
|
||||
|
||||
Reference in New Issue
Block a user