diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index a40b290e3a..60e161537d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -16,6 +16,7 @@ #include "AP_RangeFinder_PulsedLightLRF.h" #include +#include extern const AP_HAL::HAL& hal; @@ -24,8 +25,10 @@ extern const AP_HAL::HAL& hal; constructor is not called until detect() returns true, so we already know that we should setup the rangefinder */ -AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : - AP_RangeFinder_Backend(_ranger, instance, _state) +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)) { } @@ -34,38 +37,44 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(RangeFinder &_range trying to take a reading on I2C. If we get a result the sensor is there. */ -bool AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_ranger, uint8_t instance) +AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_ranger, uint8_t instance, + RangeFinder::RangeFinder_State &_state) { - if (!start_reading()) { - return false; + AP_RangeFinder_PulsedLightLRF *sensor + = new AP_RangeFinder_PulsedLightLRF(_ranger, instance, _state); + + if (!sensor->start_reading()) { + delete sensor; + return nullptr; } // give time for the sensor to process the request hal.scheduler->delay(50); uint16_t reading_cm; - return get_reading(reading_cm); + + if (!sensor->get_reading(reading_cm)) { + delete sensor; + return nullptr; + } + + return sensor; } // start_reading() - ask sensor to make a range reading bool AP_RangeFinder_PulsedLightLRF::start_reading() { - // get pointer to i2c bus semaphore - AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); - - // exit immediately if we can't take the semaphore - if (i2c_sem == NULL || !i2c_sem->take(1)) { + if (!_dev->get_semaphore()->take(1)) { return false; } // send command to take reading - if (hal.i2c->writeRegister(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, - AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, - AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE) != 0) { - i2c_sem->give(); + if (!_dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, + AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE)) { + _dev->get_semaphore()->give(); return false; } // return semaphore - i2c_sem->give(); + _dev->get_semaphore()->give(); return true; } @@ -73,20 +82,14 @@ bool AP_RangeFinder_PulsedLightLRF::start_reading() // read - return last value measured by sensor bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm) { - uint8_t buff[2]; - - // get pointer to i2c bus semaphore - AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); - - // exit immediately if we can't take the semaphore - if (i2c_sem == NULL || !i2c_sem->take(1)) { + if (_dev->get_semaphore()->take(1)) { return false; } + uint8_t buff[2]; // read the high and low byte distance registers - if (hal.i2c->readRegisters(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR, - AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 2, &buff[0]) != 0) { - i2c_sem->give(); + if (!_dev->read_registers(AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, buff, sizeof(buff))) { + _dev->get_semaphore()->give(); return false; } @@ -94,7 +97,7 @@ bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm) reading_cm = ((uint16_t)buff[0]) << 8 | buff[1]; // return semaphore - i2c_sem->give(); + _dev->get_semaphore()->give(); // kick off another reading for next time // To-Do: replace this with continuous mode diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 0025e2f343..6438765258 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -3,6 +3,7 @@ #include "RangeFinder.h" #include "RangeFinder_Backend.h" +#include /* Connection diagram * @@ -32,18 +33,21 @@ class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend { public: - // constructor - AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); - // static detection function - static bool detect(RangeFinder &ranger, uint8_t instance); + static AP_RangeFinder_Backend *detect(RangeFinder &ranger, uint8_t instance, + RangeFinder::RangeFinder_State &_state); // update state void update(void); private: + // constructor + AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance, + RangeFinder::RangeFinder_State &_state); + // start a reading - static bool start_reading(void); - static bool get_reading(uint16_t &reading_cm); + bool start_reading(void); + bool get_reading(uint16_t &reading_cm); + AP_HAL::OwnPtr _dev; }; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 784902267a..441825d987 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -26,6 +26,8 @@ #include "AP_RangeFinder_Bebop.h" #include "AP_RangeFinder_MAVLink.h" +extern const AP_HAL::HAL& hal; + // table of user settable parameters const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Param: _TYPE @@ -490,12 +492,8 @@ void RangeFinder::detect_instance(uint8_t instance) } #endif if (type == RangeFinder_TYPE_PLI2C) { - if (AP_RangeFinder_PulsedLightLRF::detect(*this, instance)) { - state[instance].instance = instance; - drivers[instance] = new AP_RangeFinder_PulsedLightLRF(*this, instance, state[instance]); - return; - } - } + _add_backend(AP_RangeFinder_PulsedLightLRF::detect(*this, instance, state[instance])); + } if (type == RangeFinder_TYPE_MBI2C) { if (AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance)) { state[instance].instance = instance;