diff --git a/src/drivers/ll40ls/LidarLite.cpp b/src/drivers/ll40ls/LidarLite.cpp index 33dce6192c..ebe054fc35 100644 --- a/src/drivers/ll40ls/LidarLite.cpp +++ b/src/drivers/ll40ls/LidarLite.cpp @@ -43,130 +43,130 @@ #include LidarLite::LidarLite() : - _min_distance(LL40LS_MIN_DISTANCE), - _max_distance(LL40LS_MAX_DISTANCE), - _measure_ticks(0) + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _measure_ticks(0) { - //ctor + //ctor } LidarLite::~LidarLite() { - //dtor + //dtor } void LidarLite::set_minimum_distance(const float min) { - _min_distance = min; + _min_distance = min; } void LidarLite::set_maximum_distance(const float max) { - _max_distance = max; + _max_distance = max; } float LidarLite::get_minimum_distance() const { - return _min_distance; + return _min_distance; } float LidarLite::get_maximum_distance() const { - return _max_distance; + return _max_distance; } -uint32_t LidarLite::getMeasureTicks() const +uint32_t LidarLite::getMeasureTicks() const { - return _measure_ticks; + return _measure_ticks; } int LidarLite::ioctl(struct file *filp, int cmd, unsigned long arg) { - switch (cmd) { + switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { + case SENSORIOCSPOLLRATE: { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: - return -EINVAL; + /* zero would be bad */ + case 0: + return -EINVAL; - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } + return OK; + } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { - return -EINVAL; - } + /* check against maximum rate */ + if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + return -EINVAL; + } - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } - } - } + return OK; + } + } + } - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) { - return SENSOR_POLLRATE_MANUAL; - } + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } - return (1000 / _measure_ticks); + return (1000 / _measure_ticks); - case SENSORIOCRESET: - reset_sensor(); - return OK; + case SENSORIOCRESET: + reset_sensor(); + return OK; - case RANGEFINDERIOCSETMINIUMDISTANCE: { - set_minimum_distance(*(float *)arg); - return OK; - } - break; + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return OK; + } + break; - case RANGEFINDERIOCSETMAXIUMDISTANCE: { - set_maximum_distance(*(float *)arg); - return OK; - } - break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return OK; + } + break; - default: - return -EINVAL; - } + default: + return -EINVAL; + } } diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h index 1ea595909d..72bb22f263 100644 --- a/src/drivers/ll40ls/LidarLite.h +++ b/src/drivers/ll40ls/LidarLite.h @@ -38,7 +38,7 @@ * * Generic interface driver for the PulsedLight Lidar-Lite range finders. */ - #pragma once +#pragma once #include #include @@ -56,50 +56,50 @@ class LidarLite { public: - LidarLite(); + LidarLite(); - virtual ~LidarLite(); + virtual ~LidarLite(); - virtual int init() = 0; + virtual int init() = 0; - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - virtual void start() = 0; + virtual void start() = 0; - virtual void stop() = 0; + virtual void stop() = 0; - /** - * @brief - * Diagnostics - print some basic information about the driver. - */ - virtual void print_info() = 0; + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + virtual void print_info() = 0; - /** - * @brief - * print registers to console - */ - virtual void print_registers() = 0; + /** + * @brief + * print registers to console + */ + virtual void print_registers() = 0; protected: - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE - * and LL40LS_MAX_DISTANCE - */ - void set_minimum_distance(const float min); - void set_maximum_distance(const float max); - float get_minimum_distance() const; - float get_maximum_distance() const; + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(const float min); + void set_maximum_distance(const float max); + float get_minimum_distance() const; + float get_maximum_distance() const; - uint32_t getMeasureTicks() const; + uint32_t getMeasureTicks() const; - virtual int measure() = 0; - virtual int collect() = 0; + virtual int measure() = 0; + virtual int collect() = 0; - virtual int reset_sensor() = 0; + virtual int reset_sensor() = 0; private: - float _min_distance; - float _max_distance; - uint32_t _measure_ticks; + float _min_distance; + float _max_distance; + uint32_t _measure_ticks; }; diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 809b5c39bb..2ed07ae0b2 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -56,273 +56,274 @@ static const int ERROR = -1; LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : - I2C("LL40LS", path, bus, address, 100000), - _work(), - _reports(nullptr), - _sensor_ok(false), - _collect_phase(false), - _class_instance(-1), - _range_finder_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), - _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), - _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), - _last_distance(0), - _zero_counter(0), - _acquire_time_usec(0), - _pause_measurements(false), - _bus(bus) + I2C("LL40LS", path, bus, address, 100000), + _work(), + _reports(nullptr), + _sensor_ok(false), + _collect_phase(false), + _class_instance(-1), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), + _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), + _last_distance(0), + _zero_counter(0), + _acquire_time_usec(0), + _pause_measurements(false), + _bus(bus) { - // up the retries since the device misses the first measure attempts - _retries = 3; + // up the retries since the device misses the first measure attempts + _retries = 3; - // enable debug() calls - _debug_enabled = false; + // enable debug() calls + _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); } LidarLiteI2C::~LidarLiteI2C() { - /* make sure we are truly inactive */ - stop(); + /* make sure we are truly inactive */ + stop(); - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } - if (_class_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); - } + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_buffer_overflows); - perf_free(_sensor_resets); - perf_free(_sensor_zero_resets); + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + perf_free(_sensor_resets); + perf_free(_sensor_zero_resets); } int LidarLiteI2C::init() { - int ret = ERROR; + int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) { - goto out; - } + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } - /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); - if (_reports == nullptr) { - goto out; - } + if (_reports == nullptr) { + goto out; + } - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); - } - } + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; out: - return ret; + return ret; } int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) { - return transfer(®, 1, &val, 1); + return transfer(®, 1, &val, 1); } int LidarLiteI2C::probe() { - // cope with both old and new I2C bus address - const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + // cope with both old and new I2C bus address + const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; - // more retries for detection - _retries = 10; + // more retries for detection + _retries = 10; - for (uint8_t i=0; i 100)) { - return -EINVAL; - } + switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - irqstate_t flags = irqsave(); + irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { - irqrestore(flags); - return -ENOMEM; - } + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqrestore(flags); + irqrestore(flags); - return OK; - } + return OK; + } - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); - default: - { - int result = LidarLite::ioctl(filp, cmd, arg); + default: { + int result = LidarLite::ioctl(filp, cmd, arg); - if(result == -EINVAL) { - result = I2C::ioctl(filp, cmd, arg); - } + if (result == -EINVAL) { + result = I2C::ioctl(filp, cmd, arg); + } - return result; - } - } + return result; + } + } } ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); - int ret = 0; + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } - /* if automatic measurement is enabled */ - if (getMeasureTicks() > 0) { + /* if automatic measurement is enabled */ + if (getMeasureTicks() > 0) { - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - rbuf++; - } - } + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } - /* manual measurement - run one conversion */ - do { - _reports->flush(); + /* manual measurement - run one conversion */ + do { + _reports->flush(); - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } - /* wait for it to complete */ - usleep(LL40LS_CONVERSION_INTERVAL); + /* wait for it to complete */ + usleep(LL40LS_CONVERSION_INTERVAL); - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } - /* state machine will have generated a report, copy it out */ - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } - } while (0); + } while (0); - return ret; + return ret; } int LidarLiteI2C::measure() { - int ret; + int ret; - if (_pause_measurements) { - // we are in print_registers() and need to avoid - // acquisition to keep the I2C peripheral on the - // sensor active - return OK; - } + if (_pause_measurements) { + // we are in print_registers() and need to avoid + // acquisition to keep the I2C peripheral on the + // sensor active + return OK; + } - /* - * Send the command to begin a measurement. - */ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - ret = transfer(cmd, sizeof(cmd), nullptr, 0); + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; + ret = transfer(cmd, sizeof(cmd), nullptr, 0); - if (OK != ret) { - perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - return ret; - } + if (OK != ret) { + perf_count(_comms_errors); + debug("i2c::transfer returned %d", ret); - // remember when we sent the acquire so we can know when the - // acquisition has timed out - _acquire_time_usec = hrt_absolute_time(); - ret = OK; + // if we are getting lots of I2C transfer errors try + // resetting the sensor + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } - return ret; + return ret; + } + + // remember when we sent the acquire so we can know when the + // acquisition has timed out + _acquire_time_usec = hrt_absolute_time(); + ret = OK; + + return ret; } /* @@ -330,9 +331,9 @@ int LidarLiteI2C::measure() */ int LidarLiteI2C::reset_sensor() { - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; - int ret = transfer(cmd, sizeof(cmd), nullptr, 0); - return ret; + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; + int ret = transfer(cmd, sizeof(cmd), nullptr, 0); + return ret; } /* @@ -340,216 +341,229 @@ int LidarLiteI2C::reset_sensor() */ void LidarLiteI2C::print_registers() { - _pause_measurements = true; - printf("ll40ls registers\n"); - // wait for a while to ensure the lidar is in a ready state - usleep(50000); - for (uint8_t reg=0; reg<=0x67; reg++) { - uint8_t val = 0; - int ret = transfer(®, 1, &val, 1); - if (ret != OK) { - printf("%02x:XX ",(unsigned)reg); - } else { - printf("%02x:%02x ",(unsigned)reg, (unsigned)val); - } - if (reg % 16 == 15) { - printf("\n"); - } - } - printf("\n"); - _pause_measurements = false; + _pause_measurements = true; + printf("ll40ls registers\n"); + // wait for a while to ensure the lidar is in a ready state + usleep(50000); + + for (uint8_t reg = 0; reg <= 0x67; reg++) { + uint8_t val = 0; + int ret = transfer(®, 1, &val, 1); + + if (ret != OK) { + printf("%02x:XX ", (unsigned)reg); + + } else { + printf("%02x:%02x ", (unsigned)reg, (unsigned)val); + } + + if (reg % 16 == 15) { + printf("\n"); + } + } + + printf("\n"); + _pause_measurements = false; } int LidarLiteI2C::collect() { - int ret = -EIO; + int ret = -EIO; - /* read from the sensor */ - uint8_t val[2] = {0, 0}; + /* read from the sensor */ + uint8_t val[2] = {0, 0}; - perf_begin(_sample_perf); + perf_begin(_sample_perf); - // read the high and low byte distance registers - uint8_t distance_reg = LL40LS_DISTHIGH_REG; - ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + // read the high and low byte distance registers + uint8_t distance_reg = LL40LS_DISTHIGH_REG; + ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); - if (ret < 0) { - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { - /* - NACKs from the sensor are expected when we - read before it is ready, so only consider it - an error if more than 100ms has elapsed. - */ - debug("error reading from sensor: %d", ret); - perf_count(_comms_errors); - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - } - perf_end(_sample_perf); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - return ret; - } + if (ret < 0) { + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { + /* + NACKs from the sensor are expected when we + read before it is ready, so only consider it + an error if more than 100ms has elapsed. + */ + debug("error reading from sensor: %d", ret); + perf_count(_comms_errors); - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } + } - if (distance == 0) { - _zero_counter++; - if (_zero_counter == 20) { - /* we have had 20 zeros in a row - reset the - sensor. This is a known bad state of the - sensor where it returns 16 bits of zero for - the distance with a trailing NACK, and - keeps doing that even when the target comes - into a valid range. - */ - _zero_counter = 0; - perf_end(_sample_perf); - perf_count(_sensor_zero_resets); - return reset_sensor(); - } - } else { - _zero_counter = 0; - } + perf_end(_sample_perf); + // if we are getting lots of I2C transfer errors try + // resetting the sensor + return ret; + } - _last_distance = distance; + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.01f; /* cm to m */ + struct range_finder_report report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - } - else { - report.valid = 0; - } + if (distance == 0) { + _zero_counter++; - /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - } + if (_zero_counter == 20) { + /* we have had 20 zeros in a row - reset the + sensor. This is a known bad state of the + sensor where it returns 16 bits of zero for + the distance with a trailing NACK, and + keeps doing that even when the target comes + into a valid range. + */ + _zero_counter = 0; + perf_end(_sample_perf); + perf_count(_sensor_zero_resets); + return reset_sensor(); + } - if (_reports->force(&report)) { - perf_count(_buffer_overflows); - } + } else { + _zero_counter = 0; + } - /* notify anyone waiting for data */ - poll_notify(POLLIN); + _last_distance = distance; - ret = OK; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); - perf_end(_sample_perf); - return ret; + if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { + report.valid = 1; + + } else { + report.valid = 0; + } + + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; } void LidarLiteI2C::start() { - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_RANGEFINDER - }; - static orb_advert_t pub = -1; + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } void LidarLiteI2C::stop() { - work_cancel(HPWORK, &_work); + work_cancel(HPWORK, &_work); } void LidarLiteI2C::cycle_trampoline(void *arg) { - LidarLiteI2C *dev = (LidarLiteI2C *)arg; + LidarLiteI2C *dev = (LidarLiteI2C *)arg; - dev->cycle(); + dev->cycle(); } void LidarLiteI2C::cycle() { - /* collection phase? */ - if (_collect_phase) { + /* collection phase? */ + if (_collect_phase) { - /* try a collection */ - if (OK != collect()) { - debug("collection error"); - /* if we've been waiting more than 200ms then - send a new acquire */ - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT*2) { - _collect_phase = false; - } - } else { - /* next phase is measurement */ - _collect_phase = false; + /* try a collection */ + if (OK != collect()) { + debug("collection error"); - /* - * Is there a collect->measure gap? - */ - if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&LidarLiteI2C::cycle_trampoline, - this, - getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); - - return; - } - } - } + /* if we've been waiting more than 200ms then + send a new acquire */ + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) { + _collect_phase = false; + } - if (_collect_phase == false) { - /* measurement phase */ - if (OK != measure()) { - debug("measure error"); - } else { - /* next phase is collection. Don't switch to - collection phase until we have a successful - acquire request I2C transfer */ - _collect_phase = true; - } - } + } else { + /* next phase is measurement */ + _collect_phase = false; - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&LidarLiteI2C::cycle_trampoline, - this, - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + /* + * Is there a collect->measure gap? + */ + if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + + return; + } + } + } + + if (_collect_phase == false) { + /* measurement phase */ + if (OK != measure()) { + debug("measure error"); + + } else { + /* next phase is collection. Don't switch to + collection phase until we have a successful + acquire request I2C transfer */ + _collect_phase = true; + } + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + USEC2TICK(LL40LS_CONVERSION_INTERVAL)); } void LidarLiteI2C::print_info() { - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - perf_print_counter(_sensor_resets); - perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u ticks\n", getMeasureTicks()); - _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_last_distance, (unsigned)_last_distance); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + perf_print_counter(_sensor_resets); + perf_print_counter(_sensor_zero_resets); + printf("poll interval: %u ticks\n", getMeasureTicks()); + _reports->print_info("report queue"); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_last_distance, (unsigned)_last_distance); } diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index f889969e84..0784b1441a 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -73,91 +73,91 @@ class RingBuffer; class LidarLiteI2C : public LidarLite, public device::I2C { public: - LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); - virtual ~LidarLiteI2C(); + LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); + virtual ~LidarLiteI2C(); - virtual int init() override; + virtual int init() override; - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override; + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override; - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info() override; + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info() override; - /** - * print registers to console - */ - void print_registers() override; + /** + * print registers to console + */ + void print_registers() override; protected: - virtual int probe(); - virtual int read_reg(uint8_t reg, uint8_t &val); + virtual int probe(); + virtual int read_reg(uint8_t reg, uint8_t &val); - int measure() override; - int reset_sensor() override; + int measure() override; + int reset_sensor() override; private: - work_s _work; - RingBuffer *_reports; - bool _sensor_ok; - bool _collect_phase; - int _class_instance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + bool _collect_phase; + int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _range_finder_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - perf_counter_t _sensor_resets; - perf_counter_t _sensor_zero_resets; - uint16_t _last_distance; - uint16_t _zero_counter; - uint64_t _acquire_time_usec; - volatile bool _pause_measurements; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + perf_counter_t _sensor_resets; + perf_counter_t _sensor_zero_resets; + uint16_t _last_distance; + uint16_t _zero_counter; + uint64_t _acquire_time_usec; + volatile bool _pause_measurements; - /**< the bus the device is connected to */ - int _bus; + /**< the bus the device is connected to */ + int _bus; - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); - /** - * Stop the automatic measurement state machine. - */ - void stop(); + /** + * Stop the automatic measurement state machine. + */ + void stop(); - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void cycle(); - int collect(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int collect(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); private: - LidarLiteI2C(const LidarLiteI2C ©) = delete; - LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; + LidarLiteI2C(const LidarLiteI2C ©) = delete; + LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; }; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 50ee4880b9..7a52ab274a 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -48,40 +48,40 @@ static const int ERROR = -1; #endif LidarLitePWM::LidarLitePWM() : - _terminateRequested(false), - _pwmSub(-1), - _pwm{}, - _rangePub(-1), - _range{} + _terminateRequested(false), + _pwmSub(-1), + _pwm{}, + _rangePub(-1), + _range{} { } int LidarLitePWM::init() { - _pwmSub = orb_subscribe(ORB_ID(pwm_input)); + _pwmSub = orb_subscribe(ORB_ID(pwm_input)); - if(_pwmSub == -1) { - return ERROR; - } + if (_pwmSub == -1) { + return ERROR; + } - _range.type = RANGE_FINDER_TYPE_LASER; - _range.valid = false; - _rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range); + _range.type = RANGE_FINDER_TYPE_LASER; + _range.valid = false; + _rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range); - return OK; + return OK; } void LidarLitePWM::print_info() { - printf("poll interval: %u ticks\n", getMeasureTicks()); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_range.distance, (unsigned)_range.distance); + printf("poll interval: %u ticks\n", getMeasureTicks()); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_range.distance, (unsigned)_range.distance); } void LidarLitePWM::print_registers() { - printf("Not supported in PWM mode\n"); + printf("Not supported in PWM mode\n"); } void LidarLitePWM::start() @@ -91,52 +91,53 @@ void LidarLitePWM::start() void LidarLitePWM::stop() { - //TODO: stop measurement task - _terminateRequested = true; + //TODO: stop measurement task + _terminateRequested = true; } int LidarLitePWM::measure() { - int result = OK; + int result = OK; - _range.error_count = _pwm.error_count; - _range.maximum_distance = get_maximum_distance(); - _range.minimum_distance = get_minimum_distance(); - _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite - _range.distance_vector[0] = _range.distance; - _range.just_updated = 0; - _range.valid = true; + _range.error_count = _pwm.error_count; + _range.maximum_distance = get_maximum_distance(); + _range.minimum_distance = get_minimum_distance(); + _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite + _range.distance_vector[0] = _range.distance; + _range.just_updated = 0; + _range.valid = true; - //TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) - if(_range.distance <= 0.0f) { - _range.valid = false; - _range.error_count++; - result = ERROR; - } + //TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) + if (_range.distance <= 0.0f) { + _range.valid = false; + _range.error_count++; + result = ERROR; + } - orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); - return result; + return result; } int LidarLitePWM::collect() { - //Check PWM - bool update; - orb_check(_pwmSub, &update); - if(update) { - orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); - _range.timestamp = hrt_absolute_time(); - return OK; - } + //Check PWM + bool update; + orb_check(_pwmSub, &update); - //Timeout readings after 0.2 seconds and mark as invalid - if(hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT*2) { - _range.timestamp = hrt_absolute_time(); - _range.valid = false; - orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); - return ERROR; - } + if (update) { + orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); + _range.timestamp = hrt_absolute_time(); + return OK; + } - return EAGAIN; + //Timeout readings after 0.2 seconds and mark as invalid + if (hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT * 2) { + _range.timestamp = hrt_absolute_time(); + _range.valid = false; + orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + return ERROR; + } + + return EAGAIN; } diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 0b85d53abc..6204e0f9af 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -47,37 +47,37 @@ class LidarLitePWM : public LidarLite { public: - LidarLitePWM(); + LidarLitePWM(); - int init() override; + int init() override; - void start() override; + void start() override; - void stop() override; + void stop() override; - /** - * @brief - * Diagnostics - print some basic information about the driver. - */ - void print_info() override; + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + void print_info() override; - /** - * @brief - * print registers to console - */ - void print_registers() override; + /** + * @brief + * print registers to console + */ + void print_registers() override; protected: - int measure() override; + int measure() override; - int collect() override; + int collect() override; - void task_main_trampoline(int argc, char *argv[]); + void task_main_trampoline(int argc, char *argv[]); private: - bool _terminateRequested; - int _pwmSub; - pwm_input_s _pwm; - orb_advert_t _rangePub; - range_finder_report _range; + bool _terminateRequested; + int _pwmSub; + pwm_input_s _pwm; + orb_advert_t _rangePub; + range_finder_report _range; }; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 827143a9c2..6ef2da94e7 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -90,12 +90,16 @@ void start(int bus) { /* create the driver, attempt expansion bus first */ if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) + if (g_dev_ext != nullptr) { errx(0, "already started external"); + } + g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { delete g_dev_ext; g_dev_ext = nullptr; + if (bus == PX4_I2C_BUS_EXPANSION) { goto fail; } @@ -103,11 +107,15 @@ void start(int bus) } #ifdef PX4_I2C_BUS_ONBOARD + /* if this failed, attempt onboard sensor */ if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) + if (g_dev_int != nullptr) { errx(0, "already started internal"); + } + g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { /* tear down the failing onboard instance */ delete g_dev_int; @@ -117,44 +125,54 @@ void start(int bus) goto fail; } } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { goto fail; } } + #endif /* set the poll rate to default, starts automatic data collection */ if (g_dev_int != nullptr) { int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); - if (fd == -1) { - goto fail; - } + + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); close(fd); + if (ret < 0) { goto fail; } - } + } if (g_dev_ext != nullptr) { int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); - if (fd == -1) { - goto fail; - } + + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); close(fd); + if (ret < 0) { goto fail; } - } + } exit(0); fail: + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -168,7 +186,8 @@ fail: */ void stop(int bus) { - LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); + LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext); + if (*g_dev != nullptr) { delete *g_dev; *g_dev = nullptr; @@ -191,7 +210,7 @@ test(int bus) struct range_finder_report report; ssize_t sz; int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); int fd = open(path, O_RDONLY); @@ -254,7 +273,7 @@ test(int bus) void reset(int bus) { - const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); int fd = open(path, O_RDONLY); if (fd < 0) { @@ -278,7 +297,8 @@ reset(int bus) void info(int bus) { - LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); + LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -295,7 +315,8 @@ info(int bus) void regdump(int bus) { - LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); + LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -328,13 +349,16 @@ ll40ls_main(int argc, char *argv[]) while ((ch = getopt(argc, argv, "XI")) != EOF) { switch (ch) { #ifdef PX4_I2C_BUS_ONBOARD + case 'I': bus = PX4_I2C_BUS_ONBOARD; break; #endif + case 'X': bus = PX4_I2C_BUS_EXPANSION; break; + default: ll40ls::usage(); exit(0);