Browse Source

AP_InertialSensor: check for NULL device pointer in probe()

handle missing devices
mission-4.1.18
Andrew Tridgell 8 years ago
parent
commit
bd84e592f8
  1. 3
      libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
  2. 3
      libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
  3. 3
      libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
  4. 3
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
  5. 6
      libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

3
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp

@ -131,6 +131,9 @@ AP_InertialSensor_Backend * @@ -131,6 +131,9 @@ AP_InertialSensor_Backend *
AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev)
{
if (!dev) {
return nullptr;
}
auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev));
if (!sensor) {

3
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@ -107,6 +107,9 @@ AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D() @@ -107,6 +107,9 @@ AP_InertialSensor_L3G4200D::~AP_InertialSensor_L3G4200D()
AP_InertialSensor_Backend *AP_InertialSensor_L3G4200D::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
if (!dev) {
return nullptr;
}
AP_InertialSensor_L3G4200D *sensor
= new AP_InertialSensor_L3G4200D(imu, std::move(dev));
if (!sensor || !sensor->_init_sensor()) {

3
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@ -397,6 +397,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_ @@ -397,6 +397,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_
AP_HAL::OwnPtr<AP_HAL::SPIDevice> 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,

3
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@ -261,6 +261,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &i @@ -261,6 +261,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &i
AP_HAL::OwnPtr<AP_HAL::I2CDevice> 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()) {

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@ -217,6 +217,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i @@ -217,6 +217,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i
AP_HAL::OwnPtr<AP_HAL::I2CDevice> 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 @@ -233,6 +236,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_InertialSensor_MPU9250 *sensor;
dev->set_read_flag(0x80);

Loading…
Cancel
Save