From 0c7dd99d7ec4dea597ee9854aab7452339ffd6f2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Nov 2016 08:58:46 +1100 Subject: [PATCH] AP_RangeFinder: support LidarLite V2 using in-tree driver needs to run in continuous mode --- .../AP_RangeFinder_PulsedLightLRF.cpp | 184 ++++++++++++------ .../AP_RangeFinder_PulsedLightLRF.h | 33 ++-- libraries/AP_RangeFinder/RangeFinder.cpp | 9 +- libraries/AP_RangeFinder/RangeFinder.h | 2 +- 4 files changed, 143 insertions(+), 85 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 0eb532b647..407cf6d6b8 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -15,108 +15,168 @@ #include "AP_RangeFinder_PulsedLightLRF.h" #include +#include #include #include extern const AP_HAL::HAL& hal; +/* LL40LS Registers addresses */ +#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ +#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */ +#define LL40LS_COUNT 0x11 +#define LL40LS_HW_VERSION 0x41 +#define LL40LS_INTERVAL 0x45 +#define LL40LS_SW_VERSION 0x4f + +// bit values +#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ +#define LL40LS_AUTO_INCREMENT 0x80 +#define LL40LS_COUNT_CONTINUOUS 0xff +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ + +// i2c address +#define LL40LS_ADDR 0x62 + /* The constructor also initializes the rangefinder. Note that this 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, +AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : AP_RangeFinder_Backend(_ranger, instance, _state) + , _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR)) { - // try external bus first - _dev = hal.i2c_mgr->get_device(1, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR); - if (!_dev) { - // then internal - _dev = hal.i2c_mgr->get_device(0, AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR); - } } /* detect if a PulsedLight rangefinder is connected. We'll detect by - trying to take a reading on I2C. If we get a result the sensor is - there. + look for the version registers */ -AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(RangeFinder &_ranger, uint8_t instance, +AP_RangeFinder_Backend *AP_RangeFinder_PulsedLightLRF::detect(uint8_t bus, RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) { AP_RangeFinder_PulsedLightLRF *sensor - = new AP_RangeFinder_PulsedLightLRF(_ranger, instance, _state); - if (!sensor || !sensor->start_reading()) { + = new AP_RangeFinder_PulsedLightLRF(bus, _ranger, instance, _state); + if (!sensor || + !sensor->init()) { delete sensor; return nullptr; } - // give time for the sensor to process the request - hal.scheduler->delay(50); - uint16_t 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() +/* + called at 50Hz + */ +bool AP_RangeFinder_PulsedLightLRF::timer(void) { - if (!_dev || !_dev->get_semaphore()->take(1)) { - return false; + if (check_reg_counter++ == 10) { + check_reg_counter = 0; + if (!_dev->check_next_register()) { + // re-send the acquire. this handles the case of power + // cycling while running in continuous mode + _dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE); + } } - // send command to take reading - bool ret = _dev->write_register(AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, - AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE); - _dev->get_semaphore()->give(); - - return ret; + switch (phase) { + case PHASE_MEASURE: + if (_dev->write_register(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE)) { + phase = PHASE_COLLECT; + } + break; + + case PHASE_COLLECT: { + be16_t val; + // read the high and low byte distance registers + if (_dev->read_registers(LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT, (uint8_t*)&val, sizeof(val))) { + state.distance_cm = be16toh(val); + update_status(); + } else { + set_status(RangeFinder::RangeFinder_NoData); + } + if (!v2_hardware) { + // for v2 hw we use continuous mode + phase = PHASE_MEASURE; + } + break; + } + } + return true; } -// read - return last value measured by sensor -bool AP_RangeFinder_PulsedLightLRF::get_reading(uint16_t &reading_cm) -{ - be16_t val; - if (!_dev->get_semaphore()->take(1)) { - return false; - } +/* + a table of settings for a lidar + */ +struct settings_table { + uint8_t reg; + uint8_t value; +}; - // read the high and low byte distance registers - bool ret = _dev->read_registers(AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, - (uint8_t *) &val, sizeof(val)); - _dev->get_semaphore()->give(); +/* + register setup table for V1 Lidar + */ +static const struct settings_table settings_v1[] = { + { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }, +}; + +/* + register setup table for V2 Lidar + */ +static const struct settings_table settings_v2[] = { + { LL40LS_INTERVAL, 0x28 }, // 0x28 == 50Hz + { LL40LS_COUNT, LL40LS_COUNT_CONTINUOUS }, + { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }, +}; - if (!ret) { +/* + initialise the sensor to required settings + */ +bool AP_RangeFinder_PulsedLightLRF::init(void) +{ + if (!_dev || !_dev->get_semaphore()->take(0)) { return false; } + _dev->set_retries(3); + + if (!(_dev->read_registers(LL40LS_HW_VERSION, &hw_version, 1) && + hw_version > 0 && + _dev->read_registers(LL40LS_SW_VERSION, &sw_version, 1) && + sw_version > 0)) { + printf("PulsedLightI2C: bad version 0x%02x 0x%02x\n", (unsigned)hw_version, (unsigned)sw_version); + // invalid version information + _dev->get_semaphore()->give(); + return false; + } + _dev->get_semaphore()->give(); - // combine results into distance - reading_cm = be16toh(val); - - // kick off another reading for next time - // To-Do: replace this with continuous mode - hal.scheduler->delay_microseconds(200); - start_reading(); + v2_hardware = (hw_version >= 0x15); + + const struct settings_table *table; + uint8_t num_settings; + + if (v2_hardware) { + table = settings_v2; + num_settings = sizeof(settings_v2) / sizeof(settings_table); + phase = PHASE_COLLECT; + } else { + table = settings_v1; + num_settings = sizeof(settings_v1) / sizeof(settings_table); + phase = PHASE_MEASURE; + } + + _dev->setup_checked_registers(num_settings); + + for (uint8_t i = 0; i < num_settings; i++) { + _dev->write_register(table[i].reg, table[i].value, true); + } + _dev->register_periodic_callback(20000, + FUNCTOR_BIND_MEMBER(&AP_RangeFinder_PulsedLightLRF::timer, bool)); return true; } -/* - update the state of the sensor -*/ -void AP_RangeFinder_PulsedLightLRF::update(void) -{ - if (get_reading(state.distance_cm)) { - // update range_valid state based on distance measured - update_status(); - } else { - set_status(RangeFinder::RangeFinder_NoData); - } -} diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index 3aa9351325..ae21c73070 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -14,39 +14,34 @@ * ------------------------------------------------------------------------------------ */ -// i2c address -#define AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR 0x62 - -// min and max distances -#define AP_RANGEFINDER_PULSEDLIGHTLRF_MIN_DISTANCE 0 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_MAX_DISTANCE 1400 - -// registers -#define AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG 0x00 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG 0x8f // high byte of distance measurement - -// command register values -#define AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE 0x04 // Varies based on sensor revision, 0x04 is newest, 0x61 is older - class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend { public: // static detection function - static AP_RangeFinder_Backend *detect(RangeFinder &ranger, uint8_t instance, + static AP_RangeFinder_Backend *detect(uint8_t bus, RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); // update state - void update(void); + void update(void) override {} private: // constructor - AP_RangeFinder_PulsedLightLRF(RangeFinder &ranger, uint8_t instance, + AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); // start a reading - bool start_reading(void); - bool get_reading(uint16_t &reading_cm); + bool init(void); + bool timer(void); + bool lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + AP_HAL::OwnPtr _dev; + + uint8_t sw_version; + uint8_t hw_version; + uint8_t check_reg_counter; + bool v2_hardware; + + enum { PHASE_MEASURE, PHASE_COLLECT } phase; }; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index cb0a772f9f..f7fff08be1 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -565,16 +565,17 @@ void RangeFinder::update(void) } } -void RangeFinder::_add_backend(AP_RangeFinder_Backend *backend) +bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend) { if (!backend) { - return; + return false; } if (num_instances == RANGEFINDER_MAX_INSTANCES) { AP_HAL::panic("Too many RANGERS backends"); } drivers[num_instances++] = backend; + return true; } /* @@ -584,7 +585,9 @@ void RangeFinder::detect_instance(uint8_t instance) { uint8_t type = _type[instance]; if (type == RangeFinder_TYPE_PLI2C) { - _add_backend(AP_RangeFinder_PulsedLightLRF::detect(*this, instance, state[instance])); + if (!_add_backend(AP_RangeFinder_PulsedLightLRF::detect(0, *this, instance, state[instance]))) { + _add_backend(AP_RangeFinder_PulsedLightLRF::detect(1, *this, instance, state[instance])); + } } if (type == RangeFinder_TYPE_MBI2C) { _add_backend(AP_RangeFinder_MaxsonarI2CXL::detect(*this, instance, state[instance])); diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 8cbe105e62..a5e9257a8f 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -206,5 +206,5 @@ private: void update_instance(uint8_t instance); void update_pre_arm_check(uint8_t instance); - void _add_backend(AP_RangeFinder_Backend *driver); + bool _add_backend(AP_RangeFinder_Backend *driver); };