From b3ed4f4b121a01847bbaf78823b1924112caa854 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 18 Mar 2021 12:36:41 +1100 Subject: [PATCH] AP_InertialSensor: fixed handling of more than 3 accel/gyro instances allows for first 3 to work without a panic --- .../AP_InertialSensor/AP_InertialSensor.cpp | 19 ++++++++++------- .../AP_InertialSensor/AP_InertialSensor.h | 4 ++-- .../AP_InertialSensor_ADIS1647x.cpp | 6 ++++-- .../AP_InertialSensor_BMI055.cpp | 6 ++++-- .../AP_InertialSensor_BMI088.cpp | 6 ++++-- .../AP_InertialSensor_BMI160.cpp | 6 ++++-- .../AP_InertialSensor_ExternalAHRS.cpp | 21 ++++++++++++------- .../AP_InertialSensor_ExternalAHRS.h | 1 + .../AP_InertialSensor_HIL.cpp | 5 +++-- .../AP_InertialSensor_Invensense.cpp | 21 +++++++------------ .../AP_InertialSensor_Invensensev2.cpp | 21 ++++++++----------- .../AP_InertialSensor_Invensensev3.cpp | 6 ++++-- .../AP_InertialSensor_L3G4200D.cpp | 8 ++++--- .../AP_InertialSensor_LSM9DS0.cpp | 6 ++++-- .../AP_InertialSensor_LSM9DS1.cpp | 6 ++++-- .../AP_InertialSensor_RST.cpp | 6 ++++-- .../AP_InertialSensor_SITL.cpp | 10 +++++---- 17 files changed, 91 insertions(+), 67 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 6d5ccc61df..04b40df5a2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -676,11 +676,11 @@ AP_InertialSensor *AP_InertialSensor::get_singleton() /* register a new gyro instance */ -uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz, - uint32_t id) +bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id) { if (_gyro_count == INS_MAX_INSTANCES) { - AP_HAL::panic("Too many gyros"); + gcs().send_text(MAV_SEVERITY_WARNING, "Failed to register gyro id %u", unsigned(id)); + return false; } _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz; @@ -704,17 +704,19 @@ uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz, } #endif - return _gyro_count++; + instance = _gyro_count++; + + return true; } /* register a new accel instance */ -uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz, - uint32_t id) +bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id) { if (_accel_count == INS_MAX_INSTANCES) { - AP_HAL::panic("Too many accels"); + gcs().send_text(MAV_SEVERITY_WARNING, "Failed to register accel id %u", unsigned(id)); + return false; } _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz; @@ -742,7 +744,8 @@ uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz, _accel_id[_accel_count].save(); #endif - return _accel_count++; + instance = _accel_count++; + return true; } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 7c2f1d0c9f..1575e5538d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -96,8 +96,8 @@ public: /// Register a new gyro/accel driver, allocating an instance /// number - uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id); - uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id); + bool register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id); + bool register_accel(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id); // a function called by the main thread at the main loop rate: void periodic(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp index a2646b87d6..289954cfa7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp @@ -89,8 +89,10 @@ AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu, void AP_InertialSensor_ADIS1647x::start() { - accel_instance = _imu.register_accel(BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)); - gyro_instance = _imu.register_gyro(BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)); + if (!_imu.register_accel(accel_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) || + !_imu.register_gyro(gyro_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) { + return; + } // setup sensor rotations from probe() set_gyro_orientation(gyro_instance, rotation); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp index d3ad9e6e14..07fe5f203b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp @@ -101,8 +101,10 @@ AP_InertialSensor_BMI055::probe(AP_InertialSensor &imu, void AP_InertialSensor_BMI055::start() { - accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055)); - gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055)); + if (!_imu.register_accel(accel_instance, ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055)) || + !_imu.register_gyro(gyro_instance, GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055))) { + return; + } // setup sensor rotations from probe() set_gyro_orientation(gyro_instance, rotation); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp index 785e851ef0..f6df2e247f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp @@ -97,8 +97,10 @@ AP_InertialSensor_BMI088::probe(AP_InertialSensor &imu, void AP_InertialSensor_BMI088::start() { - accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088)); - gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088)); + if (!_imu.register_accel(accel_instance, ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088)) || + !_imu.register_gyro(gyro_instance, GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088))) { + return; + } // setup sensor rotations from probe() set_gyro_orientation(gyro_instance, rotation); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp index 84476c3497..c9dcb031f9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -178,8 +178,10 @@ void AP_InertialSensor_BMI160::start() _dev->get_semaphore()->give(); - _accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)); - _gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)); + if (!_imu.register_accel(_accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) || + !_imu.register_gyro(_gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160))) { + return; + } /* Call _poll_data() at 1kHz */ _dev->register_periodic_callback(1000, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.cpp index d88a0ca54a..3f2a966f48 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.cpp @@ -14,6 +14,9 @@ AP_InertialSensor_ExternalAHRS::AP_InertialSensor_ExternalAHRS(AP_InertialSensor void AP_InertialSensor_ExternalAHRS::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) { + if (!started) { + return; + } Vector3f accel = pkt.accel; Vector3f gyro = pkt.gyro; @@ -29,18 +32,22 @@ void AP_InertialSensor_ExternalAHRS::handle_external(const AP_ExternalAHRS::ins_ bool AP_InertialSensor_ExternalAHRS::update(void) { - update_accel(accel_instance); - update_gyro(gyro_instance); - return true; + if (started) { + update_accel(accel_instance); + update_gyro(gyro_instance); + } + return started; } void AP_InertialSensor_ExternalAHRS::start() { const float rate = AP::externalAHRS().get_IMU_rate(); - gyro_instance = _imu.register_gyro(rate, - AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 1, DEVTYPE_SERIAL)); - accel_instance = _imu.register_accel(rate, - AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 2, DEVTYPE_SERIAL)); + if (_imu.register_gyro(gyro_instance, rate, + AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 1, DEVTYPE_SERIAL)) && + _imu.register_accel(accel_instance, rate, + AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 2, DEVTYPE_SERIAL))) { + started = true; + } } void AP_InertialSensor_ExternalAHRS::accumulate() diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h b/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h index dd5c4f4278..626deded31 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h @@ -23,6 +23,7 @@ private: uint8_t gyro_instance; uint8_t accel_instance; const uint8_t serial_port; + bool started; }; #endif // HAL_EXTERNAL_AHRS_ENABLED diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index 8b6477c7bf..56d1d114d3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -27,8 +27,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu bool AP_InertialSensor_HIL::_init_sensor(void) { // grab the used instances - _imu.register_gyro(1200, 1); - _imu.register_accel(1200, 1); + uint8_t instance; + _imu.register_gyro(instance, 1200, 1); + _imu.register_accel(instance, 1200, 1); _imu.set_hil_mode(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index ab136acd92..d87e3aa991 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -196,7 +196,7 @@ bool AP_InertialSensor_Invensense::_has_auxiliary_bus() void AP_InertialSensor_Invensense::start() { - _dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(_dev->get_semaphore()); // initially run the bus at low speed _dev->set_speed(AP_HAL::Device::SPEED_LOW); @@ -284,8 +284,10 @@ void AP_InertialSensor_Invensense::start() break; } - _gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev)); - _accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev)); + if (!_imu.register_gyro(_gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) || + !_imu.register_accel(_accel_instance, 1000, _dev->get_bus_id_devtype(adev))) { + return; + } // setup ODR and on-sensor filtering _set_filter_register(); @@ -387,8 +389,6 @@ void AP_InertialSensor_Invensense::start() // now that we have initialised, we set the bus speed to high _dev->set_speed(AP_HAL::Device::SPEED_HIGH); - _dev->get_semaphore()->give(); - // setup sensor rotations from probe() set_gyro_orientation(_gyro_instance, _rotation); set_accel_orientation(_accel_instance, _rotation); @@ -930,7 +930,7 @@ bool AP_InertialSensor_Invensense::_check_whoami(void) bool AP_InertialSensor_Invensense::_hardware_init(void) { - _dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(_dev->get_semaphore()); // setup for register checking. We check much less often on I2C // where the cost of the checks is higher @@ -940,7 +940,6 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) _dev->set_speed(AP_HAL::Device::SPEED_LOW); if (!_check_whoami()) { - _dev->get_semaphore()->give(); return false; } @@ -998,7 +997,6 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) if (tries == 5) { hal.console->printf("Failed to boot Invensense 5 times\n"); - _dev->get_semaphore()->give(); return false; } @@ -1008,8 +1006,7 @@ bool AP_InertialSensor_Invensense::_hardware_init(void) // this avoids a sensor bug, see description above _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true); } - _dev->get_semaphore()->give(); - + return true; } @@ -1142,7 +1139,7 @@ void AP_Invensense_AuxiliaryBus::_configure_slaves() return; } - backend._dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(backend._dev->get_semaphore()); /* Enable the I2C master to slaves on the auxiliary I2C bus*/ if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) { @@ -1162,8 +1159,6 @@ void AP_Invensense_AuxiliaryBus::_configure_slaves() backend._register_write(MPUREG_I2C_MST_DELAY_CTRL, BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN | BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN); - - backend._dev->get_semaphore()->give(); } int AP_Invensense_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index 07a39f7baf..7a78a1b25e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -156,7 +156,7 @@ bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus() void AP_InertialSensor_Invensensev2::start() { - _dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(_dev->get_semaphore()); // initially run the bus at low speed _dev->set_speed(AP_HAL::Device::SPEED_LOW); @@ -192,8 +192,11 @@ void AP_InertialSensor_Invensensev2::start() break; } - _gyro_instance = _imu.register_gyro(1125, _dev->get_bus_id_devtype(gdev)); - _accel_instance = _imu.register_accel(1125, _dev->get_bus_id_devtype(adev)); + if (!_imu.register_gyro(_gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) || + !_imu.register_accel(_accel_instance, 1125, _dev->get_bus_id_devtype(adev))) { + return; + } + // setup on-sensor filtering and scaling _set_filter_and_scaling(); @@ -219,8 +222,6 @@ void AP_InertialSensor_Invensensev2::start() // now that we have initialised, we set the bus speed to high _dev->set_speed(AP_HAL::Device::SPEED_HIGH); - _dev->get_semaphore()->give(); - // setup sensor rotations from probe() set_gyro_orientation(_gyro_instance, _rotation); set_accel_orientation(_accel_instance, _rotation); @@ -664,7 +665,7 @@ bool AP_InertialSensor_Invensensev2::_check_whoami(void) bool AP_InertialSensor_Invensensev2::_hardware_init(void) { - _dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(_dev->get_semaphore()); // disabled setup of checked registers as it can't cope with bank switching _dev->setup_checked_registers(7, _dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20); @@ -674,7 +675,6 @@ bool AP_InertialSensor_Invensensev2::_hardware_init(void) _dev->set_speed(AP_HAL::Device::SPEED_LOW); if (!_check_whoami()) { - _dev->get_semaphore()->give(); return false; } @@ -725,15 +725,13 @@ bool AP_InertialSensor_Invensensev2::_hardware_init(void) if (tries == 5) { hal.console->printf("Failed to boot Invensense 5 times\n"); - _dev->get_semaphore()->give(); return false; } if (_inv2_type == Invensensev2_ICM20649) { _clip_limit = 29.5f * GRAVITY_MSS; } - _dev->get_semaphore()->give(); - + return true; } @@ -860,7 +858,7 @@ void AP_Invensensev2_AuxiliaryBus::_configure_slaves() { auto &backend = AP_InertialSensor_Invensensev2::from(_ins_backend); - backend._dev->get_semaphore()->take_blocking(); + WITH_SEMAPHORE(backend._dev->get_semaphore()); /* Enable the I2C master to slaves on the auxiliary I2C bus*/ if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) { @@ -881,7 +879,6 @@ void AP_Invensensev2_AuxiliaryBus::_configure_slaves() BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN | BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN); - backend._dev->get_semaphore()->give(); } int AP_Invensensev2_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 554e03c4d0..b38288651c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -161,8 +161,10 @@ void AP_InertialSensor_Invensensev3::start() break; } - gyro_instance = _imu.register_gyro(INV3_ODR, dev->get_bus_id_devtype(devtype)); - accel_instance = _imu.register_accel(INV3_ODR, dev->get_bus_id_devtype(devtype)); + if (!_imu.register_gyro(gyro_instance, INV3_ODR, dev->get_bus_id_devtype(devtype)) || + !_imu.register_accel(accel_instance, INV3_ODR, dev->get_bus_id_devtype(devtype))) { + return; + } // setup on-sensor filtering and scaling set_filter_and_scaling(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 878a5e8a03..1bc8040475 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -250,8 +250,10 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void) */ void AP_InertialSensor_L3G4200D::start(void) { - _gyro_instance = _imu.register_gyro(800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)); - _accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D)); + if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)) || + !_imu.register_accel(_accel_instance, 800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D))) { + return; + } // start the timer process to read samples _dev_accel->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate_accel, void)); @@ -346,4 +348,4 @@ void AP_InertialSensor_L3G4200D::_accumulate_accel (void) } } -#endif \ No newline at end of file +#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 8372f7c80b..f3eac505bb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -512,8 +512,10 @@ fail_whoami: */ void AP_InertialSensor_LSM9DS0::start(void) { - _gyro_instance = _imu.register_gyro(760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)); - _accel_instance = _imu.register_accel(1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D)); + if (!_imu.register_gyro(_gyro_instance, 760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)) || + !_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D))) { + return; + } if (whoami_g == LSM9DS0_G_WHOAMI_H) { set_gyro_orientation(_gyro_instance, _rotation_gH); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp index f5259a2a04..57a55e551b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp @@ -330,8 +330,10 @@ void AP_InertialSensor_LSM9DS1::_fifo_reset() */ void AP_InertialSensor_LSM9DS1::start(void) { - _gyro_instance = _imu.register_gyro(952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)); - _accel_instance = _imu.register_accel(952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1)); + if (!_imu.register_gyro(_gyro_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)) || + !_imu.register_accel(_accel_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1))) { + return; + } set_accel_orientation(_accel_instance, _rotation); set_gyro_orientation(_gyro_instance, _rotation); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp index 9ef2bfc32c..e1f186870f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp @@ -336,8 +336,10 @@ bool AP_InertialSensor_RST::_init_sensor(void) */ void AP_InertialSensor_RST::start(void) { - _gyro_instance = _imu.register_gyro(800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D)); - _accel_instance = _imu.register_accel(1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ)); + if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D)) || + !_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ))) { + return; + } set_gyro_orientation(_gyro_instance, _rotation_g); set_accel_orientation(_accel_instance, _rotation_a); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index 7eede63501..0084de73c1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -337,10 +337,12 @@ uint8_t AP_InertialSensor_SITL::bus_id = 0; void AP_InertialSensor_SITL::start() { - gyro_instance = _imu.register_gyro(gyro_sample_hz, - AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 1, DEVTYPE_SITL)); - accel_instance = _imu.register_accel(accel_sample_hz, - AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 2, DEVTYPE_SITL)); + if (!_imu.register_gyro(gyro_instance, gyro_sample_hz, + AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 1, DEVTYPE_SITL)) || + !_imu.register_accel(accel_instance, accel_sample_hz, + AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 2, DEVTYPE_SITL))) { + return; + } bus_id++; hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void)); }