diff --git a/src/drivers/distance_sensor/ll40ls/CMakeLists.txt b/src/drivers/distance_sensor/ll40ls/CMakeLists.txt index 216e9f825419..45fd63d64aa6 100644 --- a/src/drivers/distance_sensor/ll40ls/CMakeLists.txt +++ b/src/drivers/distance_sensor/ll40ls/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,9 +35,13 @@ px4_add_module( MAIN ll40ls COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable + STACK_MAIN + 1500 SRCS ll40ls.cpp LidarLite.cpp LidarLiteI2C.cpp LidarLitePWM.cpp + DEPENDS + drivers_rangefinder ) diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp index 68869c9e6853..4c34077e9e86 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,64 +41,35 @@ #include "LidarLite.h" -int LidarLite::ioctl(device::file_t *filp, int cmd, unsigned long arg) +LidarLite::LidarLite(uint8_t rotation) : + _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls: read")), + _sample_interval_perf(perf_alloc(PC_ELAPSED, "ll40ls: interval")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls: comms errors")), + _sensor_resets(perf_alloc(PC_COUNT, "ll40ls: resets")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls: zero resets")) { - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = (LL40LS_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* convert hz to tick interval via microseconds */ - unsigned interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < (LL40LS_CONVERSION_INTERVAL)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - reset_sensor(); - return OK; + _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); + _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V3); + _px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian +} - default: - return -EINVAL; - } +LidarLite::~LidarLite() +{ + perf_free(_sample_perf); + perf_free(_sample_interval_perf); + perf_free(_comms_errors); + perf_free(_sensor_resets); + perf_free(_sensor_zero_resets); +}; + +void +LidarLite::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_sample_interval_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_sensor_resets); + perf_print_counter(_sensor_zero_resets); + printf("poll interval: %u \n", get_measure_interval()); } diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.h b/src/drivers/distance_sensor/ll40ls/LidarLite.h index 08b1ae3f39d4..a958f6ea95fd 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,7 +41,8 @@ #pragma once #include -#include +#include +#include /* Device limits */ #define LL40LS_MIN_DISTANCE (0.05f) @@ -57,51 +58,43 @@ class LidarLite { public: - LidarLite() = default; - virtual ~LidarLite() = default; + LidarLite(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + virtual ~LidarLite(); virtual int init() = 0; - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual void start() = 0; - virtual void stop() = 0; /** * @brief * Diagnostics - print some basic information about the driver. */ - virtual void print_info() = 0; + void print_info(); /** * @brief * print registers to console */ - virtual void print_registers() = 0; - - virtual const char *get_dev_name() = 0; + virtual void print_registers() {}; 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_V3 - */ - void set_minimum_distance(const float min) { _min_distance = min; } - void set_maximum_distance(const float max) { _max_distance = max; } - float get_minimum_distance() const { return _min_distance; } - float get_maximum_distance() const { return _max_distance; } - uint32_t getMeasureInterval() const { return _measure_interval; } + uint32_t get_measure_interval() const { return _measure_interval; } + + virtual int measure() = 0; + virtual int collect() = 0; - virtual int measure() = 0; - virtual int collect() = 0; + virtual int reset_sensor() { return PX4_ERROR; }; - virtual int reset_sensor() = 0; + PX4Rangefinder _px4_rangefinder; + + perf_counter_t _sample_perf; + perf_counter_t _sample_interval_perf; + perf_counter_t _comms_errors; + perf_counter_t _sensor_resets; + perf_counter_t _sensor_zero_resets; private: - float _min_distance{LL40LS_MIN_DISTANCE}; - float _max_distance{LL40LS_MAX_DISTANCE_V3}; - uint32_t _measure_interval{0}; + + uint32_t _measure_interval{LL40LS_CONVERSION_INTERVAL}; }; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index e2be97089533..52389ba354c8 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,37 +40,15 @@ */ #include "LidarLiteI2C.h" + #include -#include -#include -#include -#include -#include -#include #include #include -LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) : - I2C("LL40LS", path, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _rotation(rotation), - _reports(nullptr), - _sensor_ok(false), - _collect_phase(false), - _class_instance(-1), - _orb_class_instance(-1), - _distance_sensor_topic(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), - _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), - _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_resets")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_zero_resets")), - _last_distance(0), - _zero_counter(0), - _acquire_time_usec(0), - _pause_measurements(false), - _hw_version(0), - _sw_version(0), - _unit_id(0) +LidarLiteI2C::LidarLiteI2C(int bus, uint8_t rotation, int address) : + LidarLite(rotation), + I2C("LL40LS", nullptr, bus, address, 100000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) { // up the retries since the device misses the first measure attempts _retries = 3; @@ -78,30 +56,11 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr LidarLiteI2C::~LidarLiteI2C() { - /* make sure we are truly inactive */ stop(); - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - if (_distance_sensor_topic != nullptr) { - orb_unadvertise(_distance_sensor_topic); - } - - 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(_sensor_resets); - perf_free(_sensor_zero_resets); } -int LidarLiteI2C::init() +int +LidarLiteI2C::init() { int ret = PX4_ERROR; @@ -110,39 +69,22 @@ int LidarLiteI2C::init() return ret; } - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); - - if (_reports == nullptr) { - return ret; - } - - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; - measure(); - _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); - - if (_distance_sensor_topic == nullptr) { - PX4_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); - } - - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; - return ret; + start(); + + return OK; } -int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) +int +LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) { return lidar_transfer(®, 1, &val, 1); } -int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val) +int +LidarLiteI2C::write_reg(uint8_t reg, uint8_t val) { const uint8_t cmd[2] = { reg, val }; return transfer(&cmd[0], 2, nullptr, 0); @@ -152,7 +94,8 @@ int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val) LidarLite specific transfer() function that avoids a stop condition with SCL low */ -int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +int +LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { if (send != nullptr && send_len > 0) { int ret = transfer(send, send_len, nullptr, 0); @@ -169,12 +112,14 @@ int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t return OK; } -int LidarLiteI2C::probe() +int +LidarLiteI2C::probe() { // cope with both old and new I2C bus address - const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + const uint8_t addresses[2] = { LL40LS_BASEADDR, LL40LS_BASEADDR_OLD }; - uint8_t id_high = 0, id_low = 0; + uint8_t id_high = 0; + uint8_t id_low = 0; // more retries for detection _retries = 10; @@ -195,8 +140,7 @@ int LidarLiteI2C::probe() goto ok; } - PX4_DEBUG("probe failed unit_id=0x%02x\n", - (unsigned)_unit_id); + PX4_DEBUG("probe failed unit_id=0x%02x", (unsigned)_unit_id); } else { /* @@ -205,13 +149,12 @@ int LidarLiteI2C::probe() */ if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 && read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) { - set_maximum_distance(LL40LS_MAX_DISTANCE_V1); + + _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V1); goto ok; } - PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x\n", - (unsigned)_hw_version, - (unsigned)_sw_version); + PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x", (unsigned)_hw_version, (unsigned)_sw_version); } } @@ -224,84 +167,9 @@ int LidarLiteI2C::probe() return reset_sensor(); } -int LidarLiteI2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - default: { - int result = LidarLite::ioctl(filp, cmd, arg); - - if (result == -EINVAL) { - result = I2C::ioctl(filp, cmd, arg); - } - - return result; - } - } -} - -ssize_t LidarLiteI2C::read(device::file_t *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct distance_sensor_s); - struct distance_sensor_s *rbuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (getMeasureInterval() > 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++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - px4_usleep(LL40LS_CONVERSION_INTERVAL); - - /* 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); - } - - } while (0); - - return ret; -} - -int LidarLiteI2C::measure() +int +LidarLiteI2C::measure() { - int ret; - if (_pause_measurements) { // we are in print_registers() and need to avoid // acquisition to keep the I2C peripheral on the @@ -313,7 +181,7 @@ int LidarLiteI2C::measure() * Send the command to begin a measurement. */ const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0); + int ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -332,18 +200,17 @@ int LidarLiteI2C::measure() // 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; + return OK; } /* reset the sensor to power on defaults plus additional configurations */ -int LidarLiteI2C::reset_sensor() +int +LidarLiteI2C::reset_sensor() { - int ret; - ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); + int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); if (ret != OK) { return ret; @@ -369,7 +236,7 @@ int LidarLiteI2C::reset_sensor() void LidarLiteI2C::print_registers() { _pause_measurements = true; - printf("ll40ls registers\n"); + PX4_INFO("registers"); // wait for a while to ensure the lidar is in a ready state px4_usleep(50000); @@ -395,16 +262,14 @@ void LidarLiteI2C::print_registers() int LidarLiteI2C::collect() { - int ret = -EIO; - /* read from the sensor */ - uint8_t val[2] = {0, 0}; + uint8_t val[2] {}; perf_begin(_sample_perf); // read the high and low byte distance registers uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT; - ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val)); + int ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val)); // if the transfer failed or if the high bit of distance is // set then the distance is invalid @@ -431,8 +296,7 @@ int LidarLiteI2C::collect() } uint16_t distance_cm = (val[0] << 8) | val[1]; - float distance_m = float(distance_cm) * 1e-2f; - struct distance_sensor_s report; + const float distance_m = float(distance_cm) * 1e-2f; if (distance_cm == 0) { _zero_counter++; @@ -455,7 +319,8 @@ int LidarLiteI2C::collect() _zero_counter = 0; } - _last_distance = distance_cm; + // this should be fairly close to the end of the measurement, so the best approximation of the time + const hrt_abstime timestamp_sample = hrt_absolute_time(); /* Relative signal strength measurement, i.e. the strength of * the main signal peak compared to the general noise level*/ @@ -464,7 +329,7 @@ int LidarLiteI2C::collect() // check if the transfer failed if (ret < 0) { - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { + if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) { /* NACKs from the sensor are expected when we read before it is ready, so only consider it @@ -496,7 +361,7 @@ int LidarLiteI2C::collect() // check if the transfer failed if (ret < 0) { - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { + if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) { /* NACKs from the sensor are expected when we read before it is ready, so only consider it @@ -527,44 +392,25 @@ int LidarLiteI2C::collect() 0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW); // Step 2: Also use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements - if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW) { signal_quality = 0; } + if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW) { + signal_quality = 0; + } // Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS. - if (distance_m < LL40LS_MIN_DISTANCE) { signal_quality = 0; } - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.current_distance = distance_m; - report.min_distance = get_minimum_distance(); - report.max_distance = get_maximum_distance(); - report.variance = 0.0f; - report.signal_quality = signal_quality; - report.type = - distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; // the sensor is in fact a laser + sonar but there is no enum for this - report.orientation = _rotation; - report.id = 0; // TODO: set proper ID - - /* publish it, if we are the primary */ - if (_distance_sensor_topic != nullptr) { - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); + if (distance_m < LL40LS_MIN_DISTANCE) { + signal_quality = 0; } - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; + _px4_rangefinder.update(timestamp_sample, distance_m, signal_quality); perf_end(_sample_perf); - return ret; + return OK; } void LidarLiteI2C::start() { /* reset the report ring and state machine */ _collect_phase = false; - _reports->flush(); /* schedule a cycle to start things */ ScheduleNow(); @@ -586,7 +432,7 @@ void LidarLiteI2C::Run() /* if we've been waiting more than 200ms then send a new acquire */ - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) { + if (hrt_elapsed_time(&_acquire_time_usec) > (LL40LS_CONVERSION_TIMEOUT * 2)) { _collect_phase = false; } @@ -597,10 +443,10 @@ void LidarLiteI2C::Run() /* * Is there a collect->measure gap? */ - if (getMeasureInterval() > (LL40LS_CONVERSION_INTERVAL)) { + if (get_measure_interval() > LL40LS_CONVERSION_INTERVAL) { /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(getMeasureInterval() - LL40LS_CONVERSION_INTERVAL); + ScheduleDelayed(get_measure_interval() - LL40LS_CONVERSION_INTERVAL); return; } @@ -623,20 +469,3 @@ void LidarLiteI2C::Run() /* schedule a fresh cycle call when the measurement is done */ ScheduleDelayed(LL40LS_CONVERSION_INTERVAL); } - -void LidarLiteI2C::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_sensor_resets); - perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u \n", getMeasureInterval()); - _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_last_distance, (unsigned)_last_distance); -} - -const char *LidarLiteI2C::get_dev_name() -{ - return get_devname(); -} diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index 343a778845ec..38fcd7afe9dc 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,94 +44,54 @@ #include -#include - #include -#include - -#include -#include - /* Configuration Constants */ -#define LL40LS_BASEADDR 0x62 /* 7-bit address */ -#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ +static constexpr uint8_t LL40LS_BASEADDR = 0x62; /* 7-bit address */ +static constexpr uint8_t LL40LS_BASEADDR_OLD = 0x42; /* previous 7-bit address */ /* LL40LS Registers addresses */ +static constexpr uint8_t LL40LS_MEASURE_REG = 0x00; /* Measure range register */ +static constexpr uint8_t LL40LS_MSRREG_RESET = 0x00; /* reset to power on defaults */ +static constexpr uint8_t LL40LS_MSRREG_ACQUIRE = + 0x04; /* Value to initiate a measurement, varies based on sensor revision */ +static constexpr uint8_t LL40LS_DISTHIGH_REG = 0x0F; /* High byte of distance register, auto increment */ +static constexpr uint8_t LL40LS_AUTO_INCREMENT = 0x80; +static constexpr uint8_t LL40LS_HW_VERSION = 0x41; +static constexpr uint8_t LL40LS_SW_VERSION = 0x4f; +static constexpr uint8_t LL40LS_SIGNAL_STRENGTH_REG = 0x0e; +static constexpr uint8_t LL40LS_PEAK_STRENGTH_REG = 0x0c; +static constexpr uint8_t LL40LS_UNIT_ID_HIGH = 0x16; +static constexpr uint8_t LL40LS_UNIT_ID_LOW = 0x17; + +static constexpr uint8_t LL40LS_SIG_COUNT_VAL_REG = 0x02; /* Maximum acquisition count register */ +static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX = 0xFF; /* Maximum acquisition count max value */ + +static constexpr int LL40LS_SIGNAL_STRENGTH_LOW = + 24; // Minimum (relative) signal strength value for accepting a measurement +static constexpr int LL40LS_PEAK_STRENGTH_LOW = 135; // Minimum peak strength raw value for accepting a measurement +static constexpr int LL40LS_PEAK_STRENGTH_HIGH = 234; // Max peak strength raw value -#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ -#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ -#define LL40LS_DISTHIGH_REG 0x0F /* High byte of distance register, auto increment */ -#define LL40LS_AUTO_INCREMENT 0x80 -#define LL40LS_HW_VERSION 0x41 -#define LL40LS_SW_VERSION 0x4f -#define LL40LS_SIGNAL_STRENGTH_REG 0x0e -#define LL40LS_PEAK_STRENGTH_REG 0x0c -#define LL40LS_UNIT_ID_HIGH 0x16 -#define LL40LS_UNIT_ID_LOW 0x17 - -#define LL40LS_SIG_COUNT_VAL_REG 0x02 /* Maximum acquisition count register */ -#define LL40LS_SIG_COUNT_VAL_MAX 0xFF /* Maximum acquisition count max value */ - -#define LL40LS_SIGNAL_STRENGTH_LOW 24 // Minimum (relative) signal strength value for accepting a measurement -#define LL40LS_PEAK_STRENGTH_LOW 135 // Minimum peak strength raw value for accepting a measurement -#define LL40LS_PEAK_STRENGTH_HIGH 234 // Max peak strength raw value class LidarLiteI2C : public LidarLite, public device::I2C, public px4::ScheduledWorkItem { public: - LidarLiteI2C(int bus, const char *path, - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, - int address = LL40LS_BASEADDR); + LidarLiteI2C(int bus, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int address = LL40LS_BASEADDR); virtual ~LidarLiteI2C(); - int init() override; - - ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info() override; - - /** - * print registers to console - */ - void print_registers() override; + int init() override; - const char *get_dev_name() override; + void print_registers() override; protected: - int probe() override; - int read_reg(uint8_t reg, uint8_t &val); - int write_reg(uint8_t reg, uint8_t val); + int probe() override; + int read_reg(uint8_t reg, uint8_t &val); + int write_reg(uint8_t reg, uint8_t val); - int measure() override; - int reset_sensor() override; + int measure() override; + int reset_sensor() override; private: - uint8_t _rotation; - ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - bool _collect_phase; - int _class_instance; - int _orb_class_instance; - - orb_advert_t _distance_sensor_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - 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; - uint8_t _hw_version; - uint8_t _sw_version; - uint16_t _unit_id; /** * LidarLite specific transfer function. This is needed @@ -144,7 +104,7 @@ class LidarLiteI2C : public LidarLite, public device::I2C, public px4::Scheduled * @return OK if the transfer was successful, -errno * otherwise. */ - int lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); + int lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len); /** * Test whether the device supported by the driver is present at a @@ -175,7 +135,17 @@ class LidarLiteI2C : public LidarLite, public device::I2C, public px4::Scheduled void Run() override; int collect() override; -private: - LidarLiteI2C(const LidarLiteI2C ©) = delete; - LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uint16_t _zero_counter{0}; + uint64_t _acquire_time_usec{0}; + + bool _pause_measurements{false}; + + uint8_t _hw_version{0}; + uint8_t _sw_version{0}; + uint16_t _unit_id{0}; + }; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp index a1340a9d2dc8..357439315818 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,200 +49,74 @@ #include #include -LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) : - CDev(path), - ScheduledWorkItem(px4::wq_configurations::hp_default), - _rotation(rotation), - _reports(nullptr), - _class_instance(-1), - _orb_class_instance(-1), - _pwmSub(-1), - _pwm{}, - _distance_sensor_topic(nullptr), - _range{}, - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")), - _read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_pwm_zero_resets")) +LidarLitePWM::LidarLitePWM(uint8_t rotation) : + LidarLite(rotation), + ScheduledWorkItem(px4::wq_configurations::hp_default) { } LidarLitePWM::~LidarLitePWM() { stop(); - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - if (_class_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); - } - - /* free perf counters */ - perf_free(_sample_perf); - perf_free(_sensor_zero_resets); } - -int LidarLitePWM::init() +int +LidarLitePWM::init() { - /* do regular cdev init */ - int ret = CDev::init(); - - if (ret != PX4_OK) { - return PX4_ERROR; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); - - if (_reports == nullptr) { - return PX4_ERROR; - } - - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - - /* get a publish handle on the distance_sensor topic */ - struct distance_sensor_s ds_report = {}; - measure(); - _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); - - if (_distance_sensor_topic == nullptr) { - PX4_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); - } + start(); return PX4_OK; } -void LidarLitePWM::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_read_errors); - perf_print_counter(_sensor_zero_resets); - PX4_INFO("poll interval: %u ", getMeasureInterval()); - - print_message(_range); -} - -void LidarLitePWM::print_registers() +void +LidarLitePWM::start() { - printf("Not supported in PWM mode\n"); + ScheduleOnInterval(get_measure_interval()); } -void LidarLitePWM::start() -{ - /* schedule a cycle to start things */ - ScheduleNow(); -} - -void LidarLitePWM::stop() +void +LidarLitePWM::stop() { ScheduleClear(); } -void LidarLitePWM::Run() +void +LidarLitePWM::Run() { measure(); - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(getMeasureInterval()); } -int LidarLitePWM::measure() +int +LidarLitePWM::measure() { perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + if (PX4_OK != collect()) { PX4_DEBUG("collection error"); - perf_count(_read_errors); + perf_count(_comms_errors); perf_end(_sample_perf); return PX4_ERROR; } - _range.timestamp = hrt_absolute_time(); - _range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - _range.max_distance = get_maximum_distance(); - _range.min_distance = get_minimum_distance(); - _range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ - _range.variance = 0.0f; - _range.orientation = _rotation; - /* TODO: set proper ID */ - _range.id = 0; + const float current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */ - if (_range.current_distance <= 0.0f) { + if (current_distance <= 0.0f) { perf_count(_sensor_zero_resets); perf_end(_sample_perf); return reset_sensor(); } - if (_distance_sensor_topic != nullptr) { - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range); - } - - _reports->force(&_range); + _px4_rangefinder.update(timestamp_sample, current_distance); - poll_notify(POLLIN); perf_end(_sample_perf); return PX4_OK; } -ssize_t LidarLitePWM::read(device::file_t *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct distance_sensor_s); - struct distance_sensor_s *rbuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (getMeasureInterval() > 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++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - - } else { - - _reports->flush(); - measure(); - - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } - } - - return ret; -} - int -LidarLitePWM::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - /* no custom ioctls implemented for now */ - default: - /* give it to the superclass */ - return LidarLite::ioctl(filp, cmd, arg); - } -} - -int LidarLitePWM::collect() +LidarLitePWM::collect() { int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY); @@ -258,22 +132,3 @@ int LidarLitePWM::collect() ::close(fd); return EAGAIN; } - -int LidarLitePWM::reset_sensor() -{ - - int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY); - - if (fd == -1) { - return PX4_ERROR; - } - - int ret = ::ioctl(fd, SENSORIOCRESET, 0); - ::close(fd); - return ret; -} - -const char *LidarLitePWM::get_dev_name() -{ - return get_devname(); -} diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h index e7809271efb5..de37f581d3bc 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,68 +46,28 @@ #include "LidarLite.h" #include - -#include -#include - -#include #include -#include - - -class LidarLitePWM : public LidarLite, public cdev::CDev, public px4::ScheduledWorkItem +class LidarLitePWM : public LidarLite, public px4::ScheduledWorkItem { public: - LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + LidarLitePWM(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); virtual ~LidarLitePWM(); int init() override; - - ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - void start() override; - void stop() override; void Run() override; - /** - * @brief - * Diagnostics - print some basic information about the driver. - */ - void print_info() override; - - /** - * @brief - * print registers to console - */ - void print_registers() override; - - const char *get_dev_name() override; - protected: int measure() override; - int collect() override; - int reset_sensor() override; - - void task_main_trampoline(int argc, char *argv[]); - private: - uint8_t _rotation; - ringbuffer::RingBuffer *_reports; - int _class_instance; - int _orb_class_instance; - int _pwmSub; - struct pwm_input_s _pwm; - orb_advert_t _distance_sensor_topic; - struct distance_sensor_s _range; - perf_counter_t _sample_perf; - perf_counter_t _read_errors; - perf_counter_t _sensor_zero_resets; + int _pwmSub{-1}; + pwm_input_s _pwm{}; + }; diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index 41e7aff1fb6a..847b9c805246 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,8 +51,6 @@ #include #include -#define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm" - enum LL40LS_BUS { LL40LS_BUS_I2C_ALL = 0, LL40LS_BUS_I2C_INTERNAL, @@ -62,20 +60,19 @@ enum LL40LS_BUS { static constexpr struct ll40ls_bus_option { enum LL40LS_BUS busid; - const char *devname; uint8_t busnum; } bus_options[] = { #ifdef PX4_I2C_BUS_EXPANSION - { LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext", PX4_I2C_BUS_EXPANSION }, + { LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION }, #endif #ifdef PX4_I2C_BUS_EXPANSION1 - { LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext1", PX4_I2C_BUS_EXPANSION1 }, + { LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION1 }, #endif #ifdef PX4_I2C_BUS_EXPANSION2 - { LL40LS_BUS_I2C_EXTERNAL, "/dev/ll40ls_ext2", PX4_I2C_BUS_EXPANSION2 }, + { LL40LS_BUS_I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION2 }, #endif #ifdef PX4_I2C_BUS_ONBOARD - { LL40LS_BUS_I2C_INTERNAL, "/dev/ll40ls_int", PX4_I2C_BUS_ONBOARD }, + { LL40LS_BUS_I2C_INTERNAL, PX4_I2C_BUS_ONBOARD }, #endif }; @@ -95,8 +92,6 @@ LidarLite *instance = nullptr; void start(enum LL40LS_BUS busid, uint8_t rotation); void stop(); -void test(); -void reset(); void info(); void regdump(); void usage(); @@ -106,15 +101,13 @@ void usage(); */ void start(enum LL40LS_BUS busid, uint8_t rotation) { - int fd, ret; - if (instance) { PX4_INFO("driver already started"); return; } if (busid == LL40LS_BUS_PWM) { - instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation); + instance = new LidarLitePWM(rotation); if (!instance) { PX4_ERR("Failed to instantiate LidarLitePWM"); @@ -133,7 +126,7 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) continue; } - instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation); + instance = new LidarLiteI2C(bus_options[i].busnum, rotation); if (!instance) { PX4_ERR("Failed to instantiate LidarLiteI2C"); @@ -153,23 +146,6 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) PX4_WARN("No LidarLite found"); return; } - - fd = px4_open(instance->get_dev_name(), O_RDONLY); - - if (fd == -1) { - PX4_ERR("Error opening fd"); - stop(); - return; - } - - ret = px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - px4_close(fd); - - if (ret < 0) { - PX4_ERR("pollrate fail"); - stop(); - return; - } } /** @@ -181,110 +157,6 @@ void stop() instance = nullptr; } -/** - * @brief Performs some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - struct distance_sensor_s report; - ssize_t sz; - int ret; - - if (!instance) { - PX4_ERR("No ll40ls driver running"); - return; - } - - int fd = px4_open(instance->get_dev_name(), O_RDONLY); - - if (fd < 0) { - PX4_ERR("Error opening fd"); - return; - } - - /* Do a simple demand read. */ - sz = px4_read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("immediate read failed"); - return; - } - - print_message(report); - - /* Start the sensor polling at 2Hz. */ - if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_ERR("failed to set 2Hz poll rate"); - return; - } - - /* Read the sensor 5 times and report each value. */ - for (unsigned i = 0; i < 5; i++) { - px4_pollfd_struct_t fds; - - /* Wait for data to be ready. */ - fds.fd = fd; - fds.events = POLLIN; - ret = px4_poll(&fds, 1, 2000); - - if (ret != 1) { - PX4_WARN("timed out waiting for sensor data"); - return; - } - - /* Now go get it. */ - sz = px4_read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_WARN("periodic read failed"); - return; - } - - print_message(report); - } - - /* Reset the sensor polling to default rate. */ - if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_WARN("failed to set default poll rate"); - } - - px4_close(fd); -} - -/** - * @brief Resets the driver. - */ -void -reset() -{ - if (!instance) { - PX4_WARN("No ll40ls driver running"); - return; - } - - int fd = px4_open(instance->get_dev_name(), O_RDONLY); - - if (fd < 0) { - PX4_ERR("Error opening fd"); - return; - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - px4_close(fd); - return; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - px4_close(fd); - return; - } -} - /** * @brief Prints status info about the driver. */ @@ -321,7 +193,7 @@ regdump() void usage() { - PX4_INFO("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]"); + PX4_INFO("missing command: try 'start', 'stop', 'info', 'info' or 'regdump' [i2c|pwm]"); PX4_INFO("options for I2C:"); PX4_INFO(" -X only external bus"); #ifdef PX4_I2C_BUS_ONBOARD @@ -392,12 +264,6 @@ ll40ls_main(int argc, char *argv[]) } else if (!strcmp(verb, "stop")) { ll40ls::stop(); - } else if (!strcmp(verb, "test")) { - ll40ls::test(); - - } else if (!strcmp(verb, "reset")) { - ll40ls::reset(); - } else if (!strcmp(verb, "regdump")) { ll40ls::regdump(); @@ -411,7 +277,7 @@ ll40ls_main(int argc, char *argv[]) return 0; } - warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + warnx("unrecognized command, try 'start', 'info' or 'regdump'"); ll40ls::usage(); return 0; } diff --git a/src/drivers/distance_sensor/ll40ls/parameters.c b/src/drivers/distance_sensor/ll40ls/parameters.c index 30f4162a229b..327c3715d166 100644 --- a/src/drivers/distance_sensor/ll40ls/parameters.c +++ b/src/drivers/distance_sensor/ll40ls/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index dbbfa015be55..81e7612b73dd 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -39,5 +39,6 @@ add_subdirectory(gyroscope) add_subdirectory(led) add_subdirectory(linux_gpio) add_subdirectory(magnetometer) +add_subdirectory(rangefinder) add_subdirectory(smbus) add_subdirectory(tone_alarm) diff --git a/src/lib/drivers/rangefinder/CMakeLists.txt b/src/lib/drivers/rangefinder/CMakeLists.txt new file mode 100644 index 000000000000..5aecf74446d8 --- /dev/null +++ b/src/lib/drivers/rangefinder/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(drivers_rangefinder PX4Rangefinder.cpp) diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp new file mode 100644 index 000000000000..5574a72315ee --- /dev/null +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "PX4Rangefinder.hpp" + +#include + +PX4Rangefinder::PX4Rangefinder(uint32_t device_id, uint8_t priority, uint8_t rotation) : + CDev(nullptr), + _distance_sensor_pub{ORB_ID(distance_sensor), priority}, + _rotation{rotation} +{ + _class_device_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + // TODO: range finders should have device ids + //_distance_sensor_pub.get().device_id = device_id; + + _distance_sensor_pub.get().orientation = rotation; +} + +PX4Rangefinder::~PX4Rangefinder() +{ + if (_class_device_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_device_instance); + } +} + +void +PX4Rangefinder::set_device_type(uint8_t devtype) +{ + // TODO: range finders should have device ids + + // // current DeviceStructure + // union device::Device::DeviceId device_id; + // device_id.devid = _distance_sensor_pub.get().device_id; + + // // update to new device type + // device_id.devid_s.devtype = devtype; + + // // copy back to report + // _distance_sensor_pub.get().device_id = device_id.devid; +} + +void +PX4Rangefinder::update(hrt_abstime timestamp, float distance, int8_t quality) +{ + distance_sensor_s &report = _distance_sensor_pub.get(); + + report.timestamp = timestamp; + report.current_distance = distance; + report.signal_quality = quality; + + _distance_sensor_pub.update(); +} + +void +PX4Rangefinder::print_status() +{ + PX4_INFO(RANGE_FINDER_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); + + print_message(_distance_sensor_pub.get()); +} diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp new file mode 100644 index 000000000000..211f605072d8 --- /dev/null +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +class PX4Rangefinder : public cdev::CDev +{ + +public: + PX4Rangefinder(uint32_t device_id, uint8_t priority, uint8_t rotation); + ~PX4Rangefinder() override; + + void set_device_type(uint8_t devtype); + //void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; } + + void set_min_distance(float distance) { _distance_sensor_pub.get().min_distance = distance; } + void set_max_distance(float distance) { _distance_sensor_pub.get().max_distance = distance; } + + void set_hfov(float fov) { _distance_sensor_pub.get().h_fov = fov; } + void set_vfov(float fov) { _distance_sensor_pub.get().v_fov = fov; } + void set_fov(float fov) { set_hfov(fov); set_vfov(fov); } + + void update(hrt_abstime timestamp, float distance, int8_t quality = -1); + + void print_status(); + +private: + + uORB::PublicationData _distance_sensor_pub; + + const uint8_t _rotation; + + int _class_device_instance{-1}; + +};