diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg index 438a11b2f0..1b5ba487ed 100644 --- a/msg/sensor_mag.msg +++ b/msg/sensor_mag.msg @@ -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 diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp index f5e92604c7..da387d23bd 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -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; } diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp index 8abe78bfa7..7e079d275d 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -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); diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp index 6428f77e09..e83124e500 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp @@ -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); diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index de470a1102..7a43f9f0a7 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -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; diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index 27e4a82aad..613d9e184d 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -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() diff --git a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp index 2222ee3e04..8055f76149 100644 --- a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp +++ b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp @@ -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() diff --git a/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp b/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp index df6d5e9d15..74ee6c4d26 100644 --- a/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp +++ b/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp @@ -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() diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.cpp b/src/drivers/magnetometer/hmc5883/HMC5883.cpp index fdf0a9353e..7263c96783 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.cpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.cpp @@ -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; diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index c65a5c95b7..73507df074 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -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() diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index cc98c5df60..b7d8512ce9 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -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() diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp index 25824e660e..52858158a8 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp @@ -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) */ } diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp index e61ec75856..5e1962f716 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp @@ -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() diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index a8ba405394..1818573b07 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -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 } diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp index bd245fa638..79c224ab2f 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -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() diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp index 8adb51818a..5da9accaa6 100644 --- a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp @@ -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() diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index d9974e92eb..0eefeedfd0 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -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)); } diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp index 24ceb46964..1767e2b27a 100644 --- a/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp @@ -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() diff --git a/src/drivers/uavcan/sensors/mag.cpp b/src/drivers/uavcan/sensors/mag.cpp index a30421cf64..25ef6fa4d0 100644 --- a/src/drivers/uavcan/sensors/mag.cpp +++ b/src/drivers/uavcan/sensors/mag.cpp @@ -144,7 +144,5 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) return PX4_ERROR; } - mag->set_external(true); - return PX4_OK; } diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index fb7d9f0922..1cb55e4ab3 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -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() diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index ea0fed2535..40907d870d 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -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); } diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 62e53f4f2f..8b6f9bf903 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -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}; }; diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index 47449faf09..f51c09b0e6 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -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_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, diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index c50d7e5900..0bc71017db 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -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); diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index ec610ef093..2b69b4412f 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -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_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, diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index f7ea9f6038..d0f5ba25a3 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -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; }; diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index e4cf35686d..b84527b2f7 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -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_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)) { diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index c3412402ce..dd898502ae 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -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); diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index 565636c926..e9269d1ad0 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -34,9 +34,18 @@ #include #include #include +#include #include #include +#if defined(CONFIG_I2C) +# include +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) +# include +#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 diff --git a/src/lib/sensor_calibration/Utilities.hpp b/src/lib/sensor_calibration/Utilities.hpp index f9ada74bc0..9e29eaa574 100644 --- a/src/lib/sensor_calibration/Utilities.hpp +++ b/src/lib/sensor_calibration/Utilities.hpp @@ -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 diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 6900be8c04..08ca4d697f 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -540,7 +540,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma uORB::SubscriptionData 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); diff --git a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp index a313fbacc6..ee24264972 100644 --- a/src/modules/mag_bias_estimator/MagBiasEstimator.cpp +++ b/src/modules/mag_bias_estimator/MagBiasEstimator.cpp @@ -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}); diff --git a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp index da29401ece..d52e26e718 100644 --- a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp +++ b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp @@ -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{}; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 562b550cbd..59219ca184 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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_sub{ORB_ID(sensor_accel), i}; + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + uORB::SubscriptionData 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(); } diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 67a1a88e15..56a1722300 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -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(); } diff --git a/src/modules/simulator/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulator/sensor_mag_sim/SensorMagSim.cpp index d98d524144..ff5e5e31aa 100644 --- a/src/modules/simulator/sensor_mag_sim/SensorMagSim.cpp +++ b/src/modules/simulator/sensor_mag_sim/SensorMagSim.cpp @@ -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()