From a372f33cc0d930332f28a62e0da30239f1b92f3a Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Tue, 12 Jul 2016 18:22:18 -0300 Subject: [PATCH] AP_RangeFinder: fix PulsedLightLRF detection - Most of the boards use bus 1 for first I2C - If the bus doesn't exist, let the detect() method fail when it call start_reading(), because _dev would be invalid --- .../AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 5a80550e36..750e4f635d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -30,7 +30,7 @@ extern const AP_HAL::HAL& hal; AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : AP_RangeFinder_Backend(_ranger, instance, _state) - , _dev(hal.i2c_mgr->get_device(0, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR)) + , _dev(hal.i2c_mgr->get_device(1, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR)) { } @@ -44,8 +44,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_rang { AP_RangeFinder_PulsedLightLRF *sensor = new AP_RangeFinder_PulsedLightLRF(_ranger, instance, _state); - - if (!sensor->start_reading()) { + if (!sensor || !sensor->start_reading()) { delete sensor; return nullptr; } @@ -64,7 +63,7 @@ AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_rang // start_reading() - ask sensor to make a range reading bool AP_RangeFinder_PulsedLightLRF::start_reading() { - if (!_dev->get_semaphore()->take(1)) { + if (!_dev || !_dev->get_semaphore()->take(1)) { return false; } @@ -72,6 +71,7 @@ bool AP_RangeFinder_PulsedLightLRF::start_reading() bool ret = _dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE); _dev->get_semaphore()->give(); + return ret; }