diff --git a/platforms/common/include/px4_platform_common/param.h b/platforms/common/include/px4_platform_common/param.h index 3a4a19410d..a93eef7790 100644 --- a/platforms/common/include/px4_platform_common/param.h +++ b/platforms/common/include/px4_platform_common/param.h @@ -146,6 +146,8 @@ public: void set(float val) { _val = val; } + bool is_default() { return param_value_is_default(handle()); } + void reset() { param_reset_no_notification(handle()); @@ -198,6 +200,8 @@ public: void set(float val) { _val = val; } + bool is_default() { return param_value_is_default(handle()); } + void reset() { param_reset_no_notification(handle()); @@ -248,6 +252,8 @@ public: void set(int32_t val) { _val = val; } + bool is_default() { return param_value_is_default(handle()); } + void reset() { param_reset_no_notification(handle()); @@ -300,6 +306,8 @@ public: void set(int32_t val) { _val = val; } + bool is_default() { return param_value_is_default(handle()); } + void reset() { param_reset_no_notification(handle()); @@ -358,6 +366,8 @@ public: void set(bool val) { _val = val; } + bool is_default() { return param_value_is_default(handle()); } + void reset() { param_reset_no_notification(handle()); diff --git a/src/lib/sensor_defaults/AccelDefaults.hpp b/src/lib/sensor_defaults/AccelDefaults.hpp new file mode 100644 index 0000000000..5ab24cf5e8 --- /dev/null +++ b/src/lib/sensor_defaults/AccelDefaults.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include + +namespace sensors +{ + +struct AccelNoiseParameters { + float noise{0.35f}; // Accelerometer noise for covariance prediction (m/s^2) + float bias_noise{0.003f}; // Process noise for IMU accelerometer bias prediction (m/s^2) + float switch_on_bias{0.2f}; // 1-sigma IMU accelerometer switch-on bias (m/s^3) + uint8_t type{DRV_DEVTYPE_UNUSED}; +}; + +AccelNoiseParameters getAccelNoise(uint32_t sensor_device_id) +{ + device::Device::DeviceId device_id; + device_id.devid = sensor_device_id; + + switch (device_id.devid_s.devtype) { + case DRV_ACC_DEVTYPE_BMI055: return AccelNoiseParameters{}; + + case DRV_ACC_DEVTYPE_BMI088: return AccelNoiseParameters{}; + + case DRV_ACC_DEVTYPE_FXOS8701C: return AccelNoiseParameters{}; + + case DRV_ACC_DEVTYPE_LSM303AGR: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ADIS16448: return AccelNoiseParameters{ .noise = 0.30f, .bias_noise = 0.002f, .switch_on_bias = 0.1f}; + + case DRV_IMU_DEVTYPE_ADIS16470: return AccelNoiseParameters{ .noise = 0.20f, .bias_noise = 0.001f, .switch_on_bias = 0.05f}; + + case DRV_IMU_DEVTYPE_ADIS16477: return AccelNoiseParameters{ .noise = 0.20f, .bias_noise = 0.001f, .switch_on_bias = 0.05f}; + + case DRV_IMU_DEVTYPE_ADIS16497: return AccelNoiseParameters{ .noise = 0.20f, .bias_noise = 0.001f, .switch_on_bias = 0.05f}; + + case DRV_IMU_DEVTYPE_ICM20602: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20608G: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20649: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20689: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20948: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM40609D: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM42605: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM42688P: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_MPU6000: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_MPU6500: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_MPU9250: return AccelNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ST_LSM9DS1_AG : return AccelNoiseParameters{}; + } + + return AccelNoiseParameters{}; +} + +}; // namespace sensors diff --git a/src/lib/sensor_defaults/GyroDefaults.hpp b/src/lib/sensor_defaults/GyroDefaults.hpp new file mode 100644 index 0000000000..864719e9b8 --- /dev/null +++ b/src/lib/sensor_defaults/GyroDefaults.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include + +namespace sensors +{ + +struct GyroNoiseParameters { + float noise{0.015f}; // Rate gyro noise for covariance prediction (rad/s) + float bias_noise{0.001f}; // Process noise for IMU rate gyro bias prediction (rad/s^2) + float switch_on_bias{0.1f}; // 1-sigma IMU gyro switch-on bias (rad/s) + uint8_t type{DRV_DEVTYPE_UNUSED}; +}; + +GyroNoiseParameters getGyroNoise(uint32_t sensor_device_id) +{ + device::Device::DeviceId device_id; + device_id.devid = sensor_device_id; + + switch (device_id.devid_s.devtype) { + case DRV_GYR_DEVTYPE_BMI055: return GyroNoiseParameters{}; + + case DRV_GYR_DEVTYPE_BMI088: return GyroNoiseParameters{}; + + case DRV_GYR_DEVTYPE_FXAS2100C: return GyroNoiseParameters{}; + + case DRV_GYR_DEVTYPE_L3GD20: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ADIS16448: return GyroNoiseParameters{ .noise = 0.012f, .bias_noise = 0.0005f, .switch_on_bias = 0.05f}; + + case DRV_IMU_DEVTYPE_ADIS16470: return GyroNoiseParameters{ .noise = 0.010f, .bias_noise = 0.0002f, .switch_on_bias = 0.02f}; + + case DRV_IMU_DEVTYPE_ADIS16477: return GyroNoiseParameters{ .noise = 0.010f, .bias_noise = 0.0002f, .switch_on_bias = 0.02f}; + + case DRV_IMU_DEVTYPE_ADIS16497: return GyroNoiseParameters{ .noise = 0.010f, .bias_noise = 0.0002f, .switch_on_bias = 0.02f}; + + case DRV_IMU_DEVTYPE_ICM20602: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20608G: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20649: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20689: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM20948: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM40609D: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM42605: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ICM42688P: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_MPU6000: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_MPU6500: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_MPU9250: return GyroNoiseParameters{}; + + case DRV_IMU_DEVTYPE_ST_LSM9DS1_AG : return GyroNoiseParameters{}; + } + + return GyroNoiseParameters{}; +} + +}; // namespace sensors diff --git a/src/lib/sensor_defaults/MagDefaults.hpp b/src/lib/sensor_defaults/MagDefaults.hpp new file mode 100644 index 0000000000..a08961be22 --- /dev/null +++ b/src/lib/sensor_defaults/MagDefaults.hpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include + +namespace sensors +{ + +static constexpr float DEFAULT_MAG_NOISE = 0.005f; // gauss + +float getMagNoise(uint32_t sensor_device_id) +{ + device::Device::DeviceId device_id; + device_id.devid = sensor_device_id; + + switch (device_id.devid_s.devtype) { + case DRV_GYR_DEVTYPE_BMI055: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_HMC5883: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_AK8963: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_LIS3MDL: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_IST8310: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_RM3100: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_QMC5883L: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_AK09916: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_IST8308: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_LIS2MDL: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_BMM150: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_ST_LSM9DS1_M: return DEFAULT_MAG_NOISE; + + case DRV_MAG_DEVTYPE_LSM303AGR: return DEFAULT_MAG_NOISE; + } + + return DEFAULT_MAG_NOISE; +} +}; // namespace sensors diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index fef13e3fe0..4cff9163ee 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -33,6 +33,10 @@ #include "EKF2.hpp" +#include +#include +#include + using namespace time_literals; using math::constrain; using matrix::Eulerf; @@ -261,6 +265,24 @@ void EKF2::Run() // update parameters from storage updateParams(); + if (_param_ekf2_acc_noise.is_default()) { + const auto noise = sensors::getAccelNoise(_device_id_accel); + _param_ekf2_acc_noise.set(noise.noise); + _param_ekf2_acc_b_noise.set(noise.bias_noise); + _param_ekf2_abias_init.set(noise.switch_on_bias); + } + + if (_param_ekf2_gyr_noise.is_default()) { + const auto noise = sensors::getGyroNoise(_device_id_gyro); + _param_ekf2_gyr_noise.set(noise.noise); + _param_ekf2_gyr_b_noise.set(noise.bias_noise); + _param_ekf2_gbias_init.set(noise.switch_on_bias); + } + + if (_param_ekf2_mag_noise.is_default()) { + _param_ekf2_mag_noise.set(sensors::getMagNoise(_device_id_mag)); + } + _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); // The airspeed scale factor correcton is only available via parameter as used by the airspeed module @@ -336,6 +358,38 @@ void EKF2::Run() imu_dt = imu.delta_angle_dt; + if (_device_id_accel != imu.accel_device_id) { + if (_param_ekf2_acc_noise.is_default()) { + const float old_noise = _param_ekf2_acc_noise.get(); + + const auto noise = sensors::getAccelNoise(imu.gyro_device_id); + _param_ekf2_acc_noise.set(noise.noise); + _param_ekf2_acc_b_noise.set(noise.bias_noise); + _param_ekf2_abias_init.set(noise.switch_on_bias); + + if (fabsf(old_noise - _param_ekf2_acc_noise.get()) > 0.f) { + PX4_INFO("%d updating accel (%d) noise %.4f -> %4f", _instance, imu.accel_device_id, (double)old_noise, + (double)_param_ekf2_acc_noise.get()); + } + } + } + + if (_device_id_gyro != imu.gyro_device_id) { + if (_param_ekf2_gyr_noise.is_default()) { + const float old_noise = _param_ekf2_gyr_noise.get(); + + const auto noise = sensors::getGyroNoise(imu.gyro_device_id); + _param_ekf2_gyr_noise.set(noise.noise); + _param_ekf2_gyr_b_noise.set(noise.bias_noise); + _param_ekf2_gbias_init.set(noise.switch_on_bias); + + if (fabsf(old_noise - _param_ekf2_gyr_noise.get()) > 0.f) { + PX4_INFO("%d updating gyro (%d) noise %.4f -> %4f", _instance, imu.gyro_device_id, (double)old_noise, + (double)_param_ekf2_gyr_noise.get()); + } + } + } + if ((_device_id_accel == 0) || (_device_id_gyro == 0)) { _device_id_accel = imu.accel_device_id; _device_id_gyro = imu.gyro_device_id; @@ -1566,6 +1620,18 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) PX4_WARN("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id); } + if (_param_ekf2_mag_noise.is_default()) { + const float old_noise = _param_ekf2_mag_noise.get(); + + const auto noise = sensors::getMagNoise(magnetometer.device_id); + _param_ekf2_mag_noise.set(noise); + + if (fabsf(old_noise - _param_ekf2_mag_noise.get()) > 0.f) { + PX4_INFO("%d updating mag (%d) noise %.4f -> %4f", _instance, magnetometer.device_id, (double)old_noise, + (double)_param_ekf2_mag_noise.get()); + } + } + reset = true; } else if (magnetometer.calibration_count > _mag_calibration_count) {