From c167364fa0a1fa6cd60800c605830f69af67cc96 Mon Sep 17 00:00:00 2001 From: Luiz Ywata Date: Thu, 14 Apr 2016 13:42:06 -0300 Subject: [PATCH] AP_RangeFinder: MaxsonarI2CXL: use I2CDevice interface --- .../AP_RangeFinder_MaxsonarI2CXL.cpp | 51 ++++++++++--------- .../AP_RangeFinder_MaxsonarI2CXL.h | 16 +++--- libraries/AP_RangeFinder/RangeFinder.cpp | 10 ++-- 3 files changed, 39 insertions(+), 38 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp index f3831bcb4a..f0a9477a32 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp @@ -25,6 +25,7 @@ #include "AP_RangeFinder_MaxsonarI2CXL.h" #include +#include extern const AP_HAL::HAL& hal; @@ -34,7 +35,8 @@ extern const AP_HAL::HAL& hal; already know that we should setup the rangefinder */ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : - AP_RangeFinder_Backend(_ranger, instance, _state) + AP_RangeFinder_Backend(_ranger, instance, _state), + _dev(hal.i2c_mgr->get_device(0, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR)) { } @@ -43,40 +45,43 @@ AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, trying to take a reading on I2C. If we get a result the sensor is there. */ -bool AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance) +AP_RangeFinder_Backend *AP_RangeFinder_MaxsonarI2CXL::detect(RangeFinder &_ranger, uint8_t instance, + RangeFinder::RangeFinder_State &_state) { - if (!start_reading()) { - return false; + AP_RangeFinder_MaxsonarI2CXL *sensor + = new AP_RangeFinder_MaxsonarI2CXL(_ranger, instance, _state); + if (!sensor || !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_MaxsonarI2CXL::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; } - uint8_t tosend[1] = - { AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING }; + uint8_t tosend[] = {AP_RANGE_FINDER_MAXSONARI2CXL_COMMAND_TAKE_RANGE_READING}; // send command to take reading - if (hal.i2c->write(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR, - 1, tosend) != 0) { - i2c_sem->give(); + if (!_dev->transfer(tosend, sizeof(tosend), nullptr, 0)) { + _dev->get_semaphore()->give(); return false; } // return semaphore - i2c_sem->give(); + _dev->get_semaphore()->give(); return true; } @@ -84,22 +89,18 @@ bool AP_RangeFinder_MaxsonarI2CXL::start_reading() // read - return last value measured by sensor bool AP_RangeFinder_MaxsonarI2CXL::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]; // take range reading and read back results - if (hal.i2c->read(AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR, 2, buff) != 0) { - i2c_sem->give(); + if (!_dev->transfer(nullptr, 0, buff, sizeof(buff))) { + _dev->get_semaphore()->give(); return false; } - i2c_sem->give(); + _dev->get_semaphore()->give(); // combine results into distance reading_cm = ((uint16_t)buff[0]) << 8 | buff[1]; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h index c38e2f2456..4008c7e9de 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.h @@ -3,6 +3,7 @@ #include "RangeFinder.h" #include "RangeFinder_Backend.h" +#include #define AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR 0x70 @@ -16,17 +17,20 @@ class AP_RangeFinder_MaxsonarI2CXL : public AP_RangeFinder_Backend { public: - // constructor - AP_RangeFinder_MaxsonarI2CXL(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_MaxsonarI2CXL(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 fb35e282f2..0e3498ed58 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -26,7 +26,7 @@ #include "AP_RangeFinder_Bebop.h" #include "AP_RangeFinder_MAVLink.h" -extern const AP_HAL::HAL& hal; +extern const AP_HAL::HAL &hal; // table of user settable parameters const AP_Param::GroupInfo RangeFinder::var_info[] = { @@ -495,18 +495,14 @@ void RangeFinder::detect_instance(uint8_t instance) _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; - drivers[instance] = new AP_RangeFinder_MaxsonarI2CXL(*this, instance, state[instance]); - return; - } + _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance, state[instance])); } if (type == RangeFinder_TYPE_LWI2C) { if (_address[instance]) { _add_backend(AP_RangeFinder_LightWareI2C::detect(*this, instance, state[instance], hal.i2c_mgr->get_device(0, _address[instance]))); } - } + } #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN if (type == RangeFinder_TYPE_PX4) { if (AP_RangeFinder_PX4::detect(*this, instance)) {