diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index 851e8b3406..42249ff09b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -131,6 +131,9 @@ AP_InertialSensor_Backend * AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu, AP_HAL::OwnPtr dev) { + if (!dev) { + return nullptr; + } auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev)); if (!sensor) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 82948b7b5c..6dfe709ca0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -107,6 +107,9 @@ AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D() AP_InertialSensor_Backend *AP_InertialSensor_L3G4200D::probe(AP_InertialSensor &imu, AP_HAL::OwnPtr dev) { + if (!dev) { + return nullptr; + } AP_InertialSensor_L3G4200D *sensor = new AP_InertialSensor_L3G4200D(imu, std::move(dev)); if (!sensor || !sensor->_init_sensor()) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 407f7b225d..c421c02af3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -397,6 +397,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_ AP_HAL::OwnPtr dev_accel, enum Rotation rotation) { + if (!dev_gyro || !dev_accel) { + return nullptr; + } AP_InertialSensor_LSM9DS0 *sensor = new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel), LSM9DS0_DRY_X_PIN, LSM9DS0_DRY_G_PIN, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index e9cd2f0b72..2d3313022e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -261,6 +261,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &i AP_HAL::OwnPtr dev, enum Rotation rotation) { + if (!dev) { + return nullptr; + } AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(imu, std::move(dev), true, rotation); if (!sensor || !sensor->_init()) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 38c9156027..e409936a5d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -217,6 +217,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i AP_HAL::OwnPtr dev, enum Rotation rotation) { + if (!dev) { + return nullptr; + } AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation); if (!sensor || !sensor->_init()) { @@ -233,6 +236,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i AP_HAL::OwnPtr dev, enum Rotation rotation) { + if (!dev) { + return nullptr; + } AP_InertialSensor_MPU9250 *sensor; dev->set_read_flag(0x80);