diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp index 1773d21a28..115be975a8 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp @@ -66,7 +66,7 @@ bool AP_OpticalFlow_PX4Flow::scan_buses(void) if (!tdev) { continue; } - if (!tdev->get_semaphore()->take(0)) { + if (!tdev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { continue; } struct i2c_integral_frame frame; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp index 6d9e14284a..50e27dd196 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp @@ -100,7 +100,7 @@ AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(OpticalFlow &_frontend) // setup the device bool AP_OpticalFlow_Pixart::setup_sensor(void) { - if (!_dev->get_semaphore()->take(0)) { + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { AP_HAL::panic("Unable to get bus semaphore"); } @@ -278,7 +278,7 @@ void AP_OpticalFlow_Pixart::timer(void) float dt = dt_us * 1.0e-6; const Vector3f &gyro = get_ahrs().get_gyro(); - if (_sem->take(0)) { + if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { integral.sum.x += burst.delta_x; integral.sum.y += burst.delta_y; integral.sum_us += dt_us; @@ -316,7 +316,7 @@ void AP_OpticalFlow_Pixart::update(void) state.device_id = 1; state.surface_quality = burst.squal; - if (integral.sum_us > 0 && _sem->take(0)) { + if (integral.sum_us > 0 && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { const Vector2f flowScaler = _flowScaler(); float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index df614d8e42..65f5c06ff9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp @@ -45,7 +45,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder &_ranger return nullptr; } - if (sensor->_dev->get_semaphore()->take(0)) { + if (sensor->_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { uint16_t reading_cm; if (!sensor->get_reading(reading_cm)) { sensor->_dev->get_semaphore()->give(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index aa71da045c..931300f98a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -68,7 +68,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_range */ bool AP_RangeFinder_MaxsonarI2CXL::_init(void) { - if (!_dev->get_semaphore()->take(0)) { + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } @@ -130,7 +130,7 @@ void AP_RangeFinder_MaxsonarI2CXL::_timer(void) { uint16_t d; if (get_reading(d)) { - if (_sem->take(0)) { + if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { distance = d; new_distance = true; _sem->give(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index e5bb46fb42..98cd9fc1ed 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -142,7 +142,7 @@ static const struct settings_table settings_v2[] = { */ bool AP_RangeFinder_PulsedLightLRF::init(void) { - if (!_dev || !_dev->get_semaphore()->take(0)) { + if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } _dev->set_retries(3); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp index 3d583b5b10..e6a0758bd6 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp @@ -67,7 +67,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_trone::detect(uint8_t bus, RangeFinder &_ */ bool AP_RangeFinder_trone::init(void) { - if (!dev->get_semaphore()->take(0)) { + if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } @@ -138,7 +138,7 @@ void AP_RangeFinder_trone::timer(void) { // take a reading uint16_t distance_cm; - if (collect(distance_cm) && _sem->take(0)) { + if (collect(distance_cm) && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { accum.sum += distance_cm; accum.count++; _sem->give(); @@ -154,7 +154,7 @@ void AP_RangeFinder_trone::timer(void) */ void AP_RangeFinder_trone::update(void) { - if (_sem->take(0)) { + if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { if (accum.count > 0) { state.distance_cm = accum.sum / accum.count; accum.sum = 0;