diff --git a/libraries/AP_Baro/AP_Baro_DPS280.cpp b/libraries/AP_Baro/AP_Baro_DPS280.cpp index 0ae79d9431..af7151a702 100644 --- a/libraries/AP_Baro/AP_Baro_DPS280.cpp +++ b/libraries/AP_Baro/AP_Baro_DPS280.cpp @@ -131,9 +131,10 @@ void AP_Baro_DPS280::set_config_registers(void) bool AP_Baro_DPS280::init() { - if (!dev || !dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!dev) { return false; } + dev->get_semaphore()->take_blocking(); // setup to allow reads on SPI if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { diff --git a/libraries/AP_Baro/AP_Baro_FBM320.cpp b/libraries/AP_Baro/AP_Baro_FBM320.cpp index c2a5d9e9e7..2b20073a29 100644 --- a/libraries/AP_Baro/AP_Baro_FBM320.cpp +++ b/libraries/AP_Baro/AP_Baro_FBM320.cpp @@ -100,9 +100,10 @@ bool AP_Baro_FBM320::read_calibration(void) bool AP_Baro_FBM320::init() { - if (!dev || !dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!dev) { return false; } + dev->get_semaphore()->take_blocking(); dev->set_speed(AP_HAL::Device::SPEED_HIGH); diff --git a/libraries/AP_Baro/AP_Baro_ICM20789.cpp b/libraries/AP_Baro/AP_Baro_ICM20789.cpp index 88989ebf1d..c05fa1c1a0 100644 --- a/libraries/AP_Baro/AP_Baro_ICM20789.cpp +++ b/libraries/AP_Baro/AP_Baro_ICM20789.cpp @@ -94,9 +94,7 @@ AP_Baro_Backend *AP_Baro_ICM20789::probe(AP_Baro &baro, */ bool AP_Baro_ICM20789::imu_spi_init(void) { - if (!dev_imu->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("PANIC: AP_Baro_ICM20789: failed to take serial semaphore ICM"); - } + dev_imu->get_semaphore()->take_blocking(); dev_imu->set_read_flag(0x80); @@ -172,9 +170,7 @@ bool AP_Baro_ICM20789::init() debug("Looking for 20789 baro\n"); - if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("PANIC: AP_Baro_ICM20789: failed to take serial semaphore for init"); - } + dev->get_semaphore()->take_blocking(); debug("Setting up IMU\n"); if (dev_imu->bus_type() != AP_HAL::Device::BUS_TYPE_I2C) { diff --git a/libraries/AP_Baro/AP_Baro_KellerLD.cpp b/libraries/AP_Baro/AP_Baro_KellerLD.cpp index 62d9bdcc3f..3baed90b9d 100644 --- a/libraries/AP_Baro/AP_Baro_KellerLD.cpp +++ b/libraries/AP_Baro/AP_Baro_KellerLD.cpp @@ -67,9 +67,7 @@ bool AP_Baro_KellerLD::_init() return false; } - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("PANIC: AP_Baro_KellerLD: failed to take serial semaphore for init"); - } + _dev->get_semaphore()->take_blocking(); // high retries for init _dev->set_retries(10); diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp index 627d4ae117..6a3f558e97 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -106,9 +106,7 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe_InvensenseIMU(AP_Baro &baro, */ bool AP_Baro_LPS2XH::_imu_i2c_init(uint8_t imu_address) { - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } + _dev->get_semaphore()->take_blocking(); // as the baro device is already locked we need to re-use it, // changing its address to match the IMU address @@ -138,9 +136,10 @@ bool AP_Baro_LPS2XH::_imu_i2c_init(uint8_t imu_address) bool AP_Baro_LPS2XH::_init() { - if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!_dev) { return false; } + _dev->get_semaphore()->take_blocking(); _has_sample = false; diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index a23a17ef6b..172fdde8f3 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -82,9 +82,7 @@ bool AP_Baro_MS56XX::_init() return false; } - if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("PANIC: AP_Baro_MS56XX: failed to take serial semaphore for init"); - } + _dev->get_semaphore()->take_blocking(); // high retries for init _dev->set_retries(10); diff --git a/libraries/AP_Baro/examples/ICM20789/ICM20789.cpp b/libraries/AP_Baro/examples/ICM20789/ICM20789.cpp index 4645729c81..fcdbd9987d 100644 --- a/libraries/AP_Baro/examples/ICM20789/ICM20789.cpp +++ b/libraries/AP_Baro/examples/ICM20789/ICM20789.cpp @@ -221,9 +221,7 @@ static bool read_calibration_data(void) // initialise baro on i2c static void i2c_init(void) { - if (!i2c_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("Failed to get baro semaphore"); - } + i2c_dev->get_semaphore()->take_blocking(); if (send_cmd16(CMD_READ_ID)) { printf("ICM20789: read ID ******\n"); @@ -249,9 +247,7 @@ static void i2c_init(void) i2c_dev->get_semaphore()->give(); printf("checking baro\n"); - if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("Failed to get device semaphore"); - } + dev->get_semaphore()->take_blocking(); uint8_t regs[3] = { 0xC0, 0xC1, 0xC2 }; for (uint8_t i=0; iget_device(HAL_INS_MPU60x0_NAME)); - if (!spi_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - AP_HAL::panic("Failed to get spi semaphore"); - } - + spi_dev->get_semaphore()->take_blocking(); i2c_dev = std::move(hal.i2c_mgr->get_device(1, 0x63)); dev = std::move(hal.i2c_mgr->get_device(1, 0x29));