fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
36 changed files with 187 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/sensor_mag.msg
View File

@ -11,6 +11,4 @@ float32 temperature # temperature in degrees Celsius
uint32 error_count
bool is_external # if true the mag is external (i.e. not built into the board)
uint8 ORB_QUEUE_LENGTH = 4

2
src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
View File

@ -535,8 +535,6 @@ bool ADIS16448::Configure()
_px4_accel.set_range(18.f * CONSTANTS_ONE_G);
_px4_gyro.set_range(math::radians(1000.f));
_px4_mag.set_external(external());
return success;
}

1
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp
View File

@ -51,7 +51,6 @@ ICM20948_AK09916::ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation) :
_px4_mag(icm20948.get_device_id(), rotation)
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
_px4_mag.set_external(icm20948.external());
// mag resolution is 1.5 milli Gauss per bit (0.15 μT/LSB)
_px4_mag.set_scale(1.5e-3f);

1
src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp
View File

@ -51,7 +51,6 @@ MPU9250_AK8963::MPU9250_AK8963(MPU9250 &mpu9250, enum Rotation rotation) :
_px4_mag(mpu9250.get_device_id(), rotation)
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963);
_px4_mag.set_external(mpu9250.external());
// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
_px4_mag.set_scale(1.5e-3f);

2
src/drivers/imu/lsm303d/LSM303D.cpp
View File

@ -64,7 +64,6 @@ LSM303D::LSM303D(const I2CSPIDriverConfig &config) :
_bad_values(perf_alloc(PC_COUNT, "lsm303d: bad_val")),
_accel_duplicates(perf_alloc(PC_COUNT, "lsm303d: acc_dupe"))
{
_px4_mag.set_external(external());
}
LSM303D::~LSM303D()
@ -558,7 +557,6 @@ LSM303D::measureMagnetometer()
_px4_mag.set_temperature(_last_temperature);
_px4_mag.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values));
_px4_mag.set_external(external());
_px4_mag.update(timestamp_sample, raw_mag_report.x, raw_mag_report.y, raw_mag_report.z);
_mag_last_measure = timestamp_sample;

1
src/drivers/magnetometer/akm/ak09916/AK09916.cpp
View File

@ -45,7 +45,6 @@ AK09916::AK09916(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
AK09916::~AK09916()

1
src/drivers/magnetometer/akm/ak8963/AK8963.cpp
View File

@ -45,7 +45,6 @@ AK8963::AK8963(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
AK8963::~AK8963()

1
src/drivers/magnetometer/bosch/bmm150/BMM150.cpp
View File

@ -40,7 +40,6 @@ BMM150::BMM150(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
BMM150::~BMM150()

3
src/drivers/magnetometer/hmc5883/HMC5883.cpp
View File

@ -48,7 +48,6 @@ HMC5883::HMC5883(device::Device *interface, const I2CSPIDriverConfig &config) :
_temperature_counter(0),
_temperature_error_count(0)
{
_px4_mag.set_external(_interface->external());
}
HMC5883::~HMC5883()
@ -367,7 +366,7 @@ int HMC5883::collect()
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
if (!_px4_mag.external()) {
if (!_interface->external()) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;

1
src/drivers/magnetometer/isentek/ist8308/IST8308.cpp
View File

@ -45,7 +45,6 @@ IST8308::IST8308(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
IST8308::~IST8308()

1
src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
View File

@ -45,7 +45,6 @@ IST8310::IST8310(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
IST8310::~IST8310()

1
src/drivers/magnetometer/lis2mdl/lis2mdl.cpp
View File

@ -52,7 +52,6 @@ LIS2MDL::LIS2MDL(device::Device *interface, const I2CSPIDriverConfig &config) :
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_measure_interval(0)
{
_px4_mag.set_external(_interface->external());
_px4_mag.set_scale(0.0015f); /* 49.152f / (2^15) */
}

1
src/drivers/magnetometer/lis3mdl/lis3mdl.cpp
View File

@ -65,7 +65,6 @@ LIS3MDL::LIS3MDL(device::Device *interface, const I2CSPIDriverConfig &config) :
_temperature_counter(0),
_temperature_error_count(0)
{
_px4_mag.set_external(_interface->external());
}
LIS3MDL::~LIS3MDL()

2
src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp
View File

@ -69,8 +69,6 @@ LSM303AGR::LSM303AGR(const I2CSPIDriverConfig &config) :
_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
_bad_values(perf_alloc(PC_COUNT, MODULE_NAME": bad_val"))
{
_px4_mag.set_external(external());
_px4_mag.set_scale(1.5f / 1000.f); // 1.5 milligauss/LSB
}

1
src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp
View File

@ -45,7 +45,6 @@ LSM9DS1_MAG::LSM9DS1_MAG(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
LSM9DS1_MAG::~LSM9DS1_MAG()

1
src/drivers/magnetometer/qmc5883l/QMC5883L.cpp
View File

@ -45,7 +45,6 @@ QMC5883L::QMC5883L(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
QMC5883L::~QMC5883L()

2
src/drivers/magnetometer/rm3100/rm3100.cpp
View File

@ -54,8 +54,6 @@ RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
_measure_interval(0),
_check_state_cnt(0)
{
_px4_mag.set_external(_interface->external());
_px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS));
}

1
src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp
View File

@ -45,7 +45,6 @@ VCM1193L::VCM1193L(const I2CSPIDriverConfig &config) :
I2CSPIDriver(config),
_px4_mag(get_device_id(), config.rotation)
{
_px4_mag.set_external(external());
}
VCM1193L::~VCM1193L()

2
src/drivers/uavcan/sensors/mag.cpp
View File

@ -144,7 +144,5 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel)
return PX4_ERROR;
}
mag->set_external(true);
return PX4_OK;
}

1
src/examples/fake_magnetometer/FakeMagnetometer.cpp
View File

@ -44,7 +44,6 @@ FakeMagnetometer::FakeMagnetometer() :
_px4_mag(0, ROTATION_NONE)
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM);
_px4_mag.set_external(false);
}
bool FakeMagnetometer::init()

2
src/lib/drivers/magnetometer/PX4Magnetometer.cpp
View File

@ -75,8 +75,6 @@ void PX4Magnetometer::update(const hrt_abstime &timestamp_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);
}

5
src/lib/drivers/magnetometer/PX4Magnetometer.hpp
View File

@ -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 &timestamp_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};
};

86
src/lib/sensor_calibration/Accelerometer.cpp
View File

@ -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,

7
src/lib/sensor_calibration/Accelerometer.hpp
View File

@ -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);

83
src/lib/sensor_calibration/Gyroscope.cpp
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,

7
src/lib/sensor_calibration/Gyroscope.hpp
View File

@ -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; };

65
src/lib/sensor_calibration/Magnetometer.cpp
View File

@ -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)) {

5
src/lib/sensor_calibration/Magnetometer.hpp
View File

@ -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);

56
src/lib/sensor_calibration/Utilities.cpp
View File

@ -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

7
src/lib/sensor_calibration/Utilities.hpp
View File

@ -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

4
src/modules/commander/mag_calibration.cpp
View File

@ -540,7 +540,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
uORB::SubscriptionData<sensor_mag_s> mag_sub{ORB_ID(sensor_mag), cur_mag};
if (mag_sub.advertised() && (mag_sub.get().device_id != 0) && (mag_sub.get().timestamp > 0)) {
worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id, mag_sub.get().is_external);
worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id);
}
// reset calibration index to match uORB numbering
@ -1016,7 +1016,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
if (mag_sub.advertised() && (mag.timestamp != 0) && (mag.device_id != 0)) {
calibration::Magnetometer cal{mag.device_id, mag.is_external};
calibration::Magnetometer cal{mag.device_id};
// force calibration index to uORB index
cal.set_calibration_index(cur_mag);

2
src/modules/mag_bias_estimator/MagBiasEstimator.cpp
View File

@ -170,7 +170,7 @@ void MagBiasEstimator::Run()
updated = true;
// apply existing mag calibration
_calibration[mag_index].set_device_id(sensor_mag.device_id, sensor_mag.is_external);
_calibration[mag_index].set_device_id(sensor_mag.device_id);
const Vector3f mag_calibrated = _calibration[mag_index].Correct(Vector3f{sensor_mag.x, sensor_mag.y, sensor_mag.z});

2
src/modules/mavlink/streams/MAG_CAL_REPORT.hpp
View File

@ -73,7 +73,7 @@ private:
sensor_mag_s sensor_mag;
if (_sensor_mag_subs[mag].update(&sensor_mag) && (sensor_mag.device_id != 0)) {
calibration::Magnetometer calibration{sensor_mag.device_id, sensor_mag.is_external};
calibration::Magnetometer calibration{sensor_mag.device_id};
if (calibration.calibrated()) {
mavlink_mag_cal_report_t msg{};

30
src/modules/sensors/sensors.cpp
View File

@ -325,41 +325,39 @@ int Sensors::parameters_update()
uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i);
if (device_id_accel != 0) {
bool external_accel = (calibration::GetCalibrationParamInt32("ACC", "ROT", i) >= 0);
calibration::Accelerometer accel_cal(device_id_accel, external_accel);
calibration::Accelerometer accel_cal(device_id_accel);
}
if (device_id_gyro != 0) {
bool external_gyro = (calibration::GetCalibrationParamInt32("GYRO", "ROT", i) >= 0);
calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro);
calibration::Gyroscope gyro_cal(device_id_gyro);
}
if (device_id_mag != 0) {
bool external_mag = (calibration::GetCalibrationParamInt32("MAG", "ROT", i) >= 0);
calibration::Magnetometer mag_cal(device_id_mag, external_mag);
calibration::Magnetometer mag_cal(device_id_mag);
}
}
// ensure calibration slots are active for the number of sensors currently available
// this to done to eliminate differences in the active set of parameters before and after sensor calibration
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (orb_exists(ORB_ID(sensor_accel), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParamInt32("ACC", "ROT", i) >= 0);
calibration::Accelerometer cal{0, external};
for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
if (sensor_accel_sub.get().device_id != 0) {
calibration::Accelerometer cal{sensor_accel_sub.get().device_id};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
if (orb_exists(ORB_ID(sensor_gyro), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParamInt32("GYRO", "ROT", i) >= 0);
calibration::Gyroscope cal{0, external};
if (sensor_gyro_sub.get().device_id != 0) {
calibration::Gyroscope cal{sensor_gyro_sub.get().device_id};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
if (orb_exists(ORB_ID(sensor_mag), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParamInt32("MAG", "ROT", i) >= 0);
calibration::Magnetometer cal{0, external};
if (sensor_mag_sub.get().device_id != 0) {
calibration::Magnetometer cal{sensor_mag_sub.get().device_id};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}

2
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -432,7 +432,7 @@ void VehicleMagnetometer::Run()
while (_sensor_sub[uorb_index].update(&report)) {
if (_calibration[uorb_index].device_id() != report.device_id) {
_calibration[uorb_index].set_device_id(report.device_id, report.is_external);
_calibration[uorb_index].set_device_id(report.device_id);
_priority[uorb_index] = _calibration[uorb_index].priority();
}

1
src/modules/simulator/sensor_mag_sim/SensorMagSim.cpp
View File

@ -43,7 +43,6 @@ SensorMagSim::SensorMagSim() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM);
_px4_mag.set_external(false);
}
SensorMagSim::~SensorMagSim()