AP_RangeFinder: use take_blocking instead of HAL_SEMAPHORE_BLOCK_FOREVER

this makes for cleaner and smaller code as the failure case is not
needed
This commit is contained in:
Andrew Tridgell 2020-01-19 08:42:34 +11:00
parent 1cee9bea28
commit 7dcb5c60b0
3 changed files with 4 additions and 7 deletions

View File

@ -75,9 +75,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder::RangeF
*/
bool AP_RangeFinder_MaxsonarI2CXL::_init(void)
{
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
_dev->get_semaphore()->take_blocking();
if (!start_reading()) {
_dev->get_semaphore()->give();

View File

@ -159,9 +159,10 @@ static const struct settings_table settings_v3hp[] = {
*/
bool AP_RangeFinder_PulsedLightLRF::init(void)
{
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (!_dev) {
return false;
}
_dev->get_semaphore()->take_blocking();
_dev->set_retries(3);
// LidarLite needs split transfers

View File

@ -72,9 +72,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_TeraRangerI2C::detect(RangeFinder::RangeF
*/
bool AP_RangeFinder_TeraRangerI2C::init(void)
{
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
dev->get_semaphore()->take_blocking();
dev->set_retries(10);