diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 57df0c09c5c6..215b9c9c29b0 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -38,68 +38,53 @@ * Driver for the vl53lxx ToF Sensor from ST Microelectronics connected via I2C. */ -#include -#include -#include -#include - -#include - -#include -#include #include -#include -#include #include -#include -#include -#include -#include -#include - -#include +#include +#include +#include #include #include -#include - -#include +#include +#include +#include #include -#include - /* Configuration Constants */ -#define VL53LXX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION +#define VL53LXX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION -#define VL53LXX_BASEADDR 0b0101001 // 7-bit address -#define VL53LXX_DEVICE_PATH "/dev/vl53lxx" +#define VL53LXX_BASEADDR 0x29 +#define VL53LXX_DEVICE_PATH "/dev/vl53lxx" /* VL53LXX Registers addresses */ -#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 -#define MSRC_CONFIG_CONTROL_REG 0x60 +#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 +#define MSRC_CONFIG_CONTROL_REG 0x60 #define FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG 0x44 -#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 -#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG 0x4F -#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG 0x4E -#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6 -#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 -#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A -#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 -#define SYSRANGE_START_REG 0x00 -#define RESULT_INTERRUPT_STATUS_REG 0x13 -#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B -#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 -#define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84 -#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B -#define RESULT_RANGE_STATUS_REG 0x14 -#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 -#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA - -#define VL53LXX_US 1000 /* 1ms */ -#define VL53LXX_SAMPLE_RATE 50000 /* 50ms */ - -#define VL53LXX_MAX_RANGING_DISTANCE 2.0f -#define VL53LXX_MIN_RANGING_DISTANCE 0.0f +#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 +#define DYNAMIC_SPAD_REF_EN_START_OFFSET_REG 0x4F +#define DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG 0x4E +#define GLOBAL_CONFIG_REF_EN_START_SELECT_REG 0xB6 +#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 +#define SYSTEM_INTERRUPT_CONFIG_GPIO_REG 0x0A +#define SYSTEM_SEQUENCE_CONFIG_REG 0x01 +#define SYSRANGE_START_REG 0x00 +#define RESULT_INTERRUPT_STATUS_REG 0x13 +#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B +#define GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG 0xB0 +#define GPIO_HV_MUX_ACTIVE_HIGH_REG 0x84 +#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B +#define RESULT_RANGE_STATUS_REG 0x14 +#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0 +#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA + +#define VL53LXX_US 1000 // 1ms +#define VL53LXX_SAMPLE_RATE 50000 // 50ms + +#define VL53LXX_MAX_RANGING_DISTANCE 2.0f +#define VL53LXX_MIN_RANGING_DISTANCE 0.0f + +#define VL53LXX_BUS_CLOCK 400000 // 400kHz bus clock speed class VL53LXX : public device::I2C, public px4::ScheduledWorkItem { @@ -111,442 +96,194 @@ class VL53LXX : public device::I2C, public px4::ScheduledWorkItem virtual int init() override; - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; + virtual ssize_t read(device::file_t *file_pointer, char *buffer, size_t buflen); /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + * Initialise the automatic measurement state machine and start it. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); protected: virtual int probe() override; private: - uint8_t _rotation; - ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - int _measure_interval; - bool _collect_phase; - bool _new_measurement; - bool _measurement_started; - - int _class_instance; - int _orb_class_instance; - - orb_advert_t _distance_sensor_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - - uint8_t _stop_variable; - /** - * Initialise the automatic measurement state machine and start it. - */ - void start(); + * Collects the most recent sensor measurement data from the i2c bus. + */ + int collect(); /** - * Stop the automatic measurement state machine. - */ - void stop(); + * Sends an i2c measure command to the sensor. + */ + int measure(); /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ void Run() override; - int measure(); - int collect(); - int readRegister(uint8_t reg_address, uint8_t &value); - int writeRegister(uint8_t reg_address, uint8_t value); + int readRegister(const uint8_t reg_address, uint8_t &value); + int readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length); - int writeRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length); - int readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length); + int writeRegister(const uint8_t reg_address, const uint8_t value); + int writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, const uint8_t length); int sensorInit(); - bool spadCalculations(); - bool sensorTuning(); - bool singleRefCalibration(uint8_t byte); -}; + int sensorTuning(); + int singleRefCalibration(const uint8_t byte); + int spadCalculations(); + bool _collect_phase{false}; + bool _measurement_started{false}; + bool _new_measurement{true}; + + int _class_instance{-1}; + int _measure_interval{VL53LXX_SAMPLE_RATE}; + int _orb_class_instance{-1}; + + uint8_t _rotation{0}; + uint8_t _stop_variable{0}; + + orb_advert_t _distance_sensor_topic{nullptr}; + + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "vl53lxx_com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "vl53lxx_read")}; + + ringbuffer::RingBuffer *_reports{nullptr}; +}; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[]); VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) : I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000), ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _rotation(rotation), - _reports(nullptr), - _sensor_ok(false), - _measure_interval(0), - _collect_phase(false), - _new_measurement(true), - _measurement_started(false), - _class_instance(-1), - _orb_class_instance(-1), - _distance_sensor_topic(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "vl53lxx_read")), - _comms_errors(perf_alloc(PC_COUNT, "vl53lxx_com_err")), - _stop_variable(0) + _rotation(rotation) { - // up the retries since the device misses the first measure attempts + // Allow 3 retries as the device typically misses the first measure attempts. I2C::_retries = 3; } VL53LXX::~VL53LXX() { - /* make sure we are truly inactive */ + // Ensure we are truly inactive. stop(); - /* free any existing reports */ + // 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); } + // Unadvertise uORB topics. + if (_distance_sensor_topic != nullptr) { + orb_unadvertise(_distance_sensor_topic); + } + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); } - int -VL53LXX::sensorInit() +VL53LXX::collect() { - uint8_t val = 0; - int ret = OK; - float rate_limit; - uint8_t rate_limit_split[2]; - - // I2C at 2.8V on sensor side of level shifter - ret |= readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val); - - if (ret != OK) { - ret = PX4_ERROR; - return ret; - } + // Read from the sensor. + uint8_t val[2] = {}; + perf_begin(_sample_perf); - ret |= writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01); + _collect_phase = false; - // set I2C to standard mode - ret |= writeRegister(0x88, 0x00); + int ret = transfer(nullptr, 0, &val[0], 2); - ret |= writeRegister(0x80, 0x01); - ret |= writeRegister(0xFF, 0x01); - ret |= writeRegister(0x00, 0x00); - ret |= readRegister(0x91, val); - ret |= writeRegister(0x00, 0x01); - ret |= writeRegister(0xFF, 0x00); - ret |= writeRegister(0x80, 0x00); - - if (ret != OK) { - ret = PX4_ERROR; + if (ret != PX4_OK) { + DEVICE_LOG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } - _stop_variable = val; - - // disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks - readRegister(MSRC_CONFIG_CONTROL_REG, val); - writeRegister(MSRC_CONFIG_CONTROL_REG, val | 0x12); - - // Set signal rate limit to 0.1 - rate_limit = 0.1 * 65536; - rate_limit_split[0] = (((uint16_t)rate_limit) >> 8); - rate_limit_split[1] = (uint16_t)rate_limit; + uint16_t distance_mm = (val[0] << 8) | val[1]; + float distance_m = distance_mm / 1000.f; - writeRegisterMulti(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG, &rate_limit_split[0], 2); - writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xFF); + struct distance_sensor_s report; + report.timestamp = hrt_absolute_time(); + report.current_distance = distance_m; + report.id = 0; // TODO: set propoer ID + report.max_distance = VL53LXX_MAX_RANGING_DISTANCE; + report.min_distance = VL53LXX_MIN_RANGING_DISTANCE; + report.orientation = _rotation; + report.signal_quality = -1; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.variance = 0.0f; + + // Publish the report data if we have a valid topic. + if (_class_instance == CLASS_DEVICE_PRIMARY) { + orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, + &_orb_class_instance, ORB_PRIO_DEFAULT); + } - spadCalculations(); + _reports->force(&report); - return ret; + // Notify anyone waiting for data. + poll_notify(POLLIN); + perf_end(_sample_perf); + return PX4_OK; } int VL53LXX::init() { - int ret = OK; - set_device_address(VL53LXX_BASEADDR); - /* do I2C init (and probe) first */ + // Initialize I2C (and probe) first. if (I2C::init() != OK) { - ret = PX4_ERROR; - goto out; + return PX4_ERROR; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { - ret = PX4_ERROR; - goto out; + return PX4_ERROR; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; - -out: - return ret; -} - -int -VL53LXX::probe() -{ - if (sensorInit() == OK) { - return OK; - } - - // not found on any address - return -EIO; -} - -int -VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - case 0: - return -EINVAL; - - 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 = (VL53LXX_SAMPLE_RATE); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - 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); - - /* 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; - } - } - } - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - - -ssize_t -VL53LXX::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 (_measure_interval > 0) { + // Get a publish handle on the obstacle distance topic. + distance_sensor_s report {}; + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &report, + &_orb_class_instance, ORB_PRIO_DEFAULT); - /* - * 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; - } - - while (!_collect_phase); - - /* 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 -VL53LXX::readRegister(uint8_t reg_address, uint8_t &value) -{ - int ret; - - /* write register address to the sensor */ - ret = transfer(®_address, sizeof(reg_address), nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - return ret; - } - - /* read from the sensor */ - ret = transfer(nullptr, 0, &value, 1); - - if (OK != ret) { - perf_count(_comms_errors); - return ret; - } - - return ret; - -} - - -int -VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) -{ - int ret; - - /* write register address to the sensor */ - ret = transfer(®_address, 1, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - return ret; - } - - /* read from the sensor */ - ret = transfer(nullptr, 0, &value[0], length); - - if (OK != ret) { - perf_count(_comms_errors); - return ret; - } - - return ret; - -} - - -int -VL53LXX::writeRegister(uint8_t reg_address, uint8_t value) -{ - int ret; - uint8_t cmd[2] = {0, 0}; - - cmd[0] = reg_address; - cmd[1] = value; - - ret = transfer(&cmd[0], 2, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - return ret; - } - - return ret; - -} - - -int -VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t *value, - uint8_t length) /* bytes are send in order as they are in the array */ -{ - /* be careful: for uint16_t to send first higher byte */ - int ret; - uint8_t cmd[7] = {0, 0, 0, 0, 0, 0, 0}; - - if (length > 6 || length < 1) { - DEVICE_LOG("VL53LXX::writeRegisterMulti length out of range"); + if (_distance_sensor_topic == nullptr) { + PX4_ERR("failed to create distance_sensor object"); return PX4_ERROR; } - cmd[0] = reg_address; - - memcpy(&cmd[1], &value[0], length); - - ret = transfer(&cmd[0], length + 1, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - return ret; - } - - return ret; - + return PX4_OK; } - int VL53LXX::measure() { - int ret = OK; - uint8_t wait_for_measurement; - uint8_t system_start; + uint8_t wait_for_measurement = 0; + uint8_t system_start = 0; - /* - * Send the command to begin a measurement. - */ + // Send the command to begin a measurement. const uint8_t cmd = RESULT_RANGE_STATUS_REG + 10; if (_new_measurement) { @@ -567,9 +304,7 @@ VL53LXX::measure() if ((system_start & 0x01) == 1) { ScheduleDelayed(VL53LXX_US); - - ret = OK; - return ret; + return PX4_OK; } else { _measurement_started = true; @@ -583,8 +318,7 @@ VL53LXX::measure() if ((system_start & 0x01) == 1) { ScheduleDelayed(VL53LXX_US); - ret = OK; - return ret; + return PX4_OK; } else { _measurement_started = true; @@ -595,96 +329,142 @@ VL53LXX::measure() if ((wait_for_measurement & 0x07) == 0) { ScheduleDelayed(VL53LXX_US); // reschedule every 1 ms until measurement is ready - ret = OK; - return ret; + return PX4_OK; } _collect_phase = true; - ret = transfer(&cmd, sizeof(cmd), nullptr, 0); + int ret = transfer(&cmd, sizeof(cmd), nullptr, 0); - if (OK != ret) { + if (ret != PX4_OK) { perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } - return ret; + return PX4_OK; } +void +VL53LXX::print_info() +{ + perf_print_counter(_comms_errors); + perf_print_counter(_sample_perf); + PX4_INFO("measure interval: %u msec", _measure_interval / 1000); + _reports->print_info("report queue"); +} int -VL53LXX::collect() +VL53LXX::probe() { - int ret = -EIO; + if (sensorInit() == PX4_OK) { + return PX4_OK; + } - /* read from the sensor */ - uint8_t val[2] = {0, 0}; + // Device not found on any address. + return -EIO; +} - perf_begin(_sample_perf); +ssize_t +VL53LXX::read(device::file_t *file_pointer, char *buffer, size_t buffer_length) +{ + size_t buffer_size = 0; + unsigned int count = buffer_length / sizeof(struct distance_sensor_s); + struct distance_sensor_s *read_buffer = reinterpret_cast(buffer); - _collect_phase = false; + // Buffer must be large enough. + if (count < 1) { + return -ENOSPC; + } - ret = transfer(nullptr, 0, &val[0], 2); + // If automatic measurement is enabled. + if (_measure_interval > 0) { - if (ret < 0) { - DEVICE_LOG("error reading from sensor: %d", ret); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } + /* + * 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(read_buffer)) { + buffer_size += sizeof(*read_buffer); + read_buffer++; + } + } - uint16_t distance_mm = (val[0] << 8) | val[1]; - float distance_m = float(distance_mm) * 1e-3f; - struct distance_sensor_s report; + // If there was no data, warn the caller. + if (buffer_size == 0) { + return -EAGAIN; + } + } - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.orientation = _rotation; + // Flush the report ring buffer. + _reports->flush(); - report.current_distance = distance_m; + // Trigger a measurement. + if (measure() != PX4_OK) { + return -EIO; + } - report.min_distance = VL53LXX_MIN_RANGING_DISTANCE; - report.max_distance = VL53LXX_MAX_RANGING_DISTANCE; - report.variance = 0.0f; - report.signal_quality = -1; + // Wait for the measurement to complete. + usleep(_measure_interval); - /* TODO: set proper ID */ - report.id = 0; + // Run the collection phase. + if (collect() != PX4_OK) { + return -EIO; + } - /* publish it, if we are the primary */ - if (_class_instance == CLASS_DEVICE_PRIMARY) { - orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &_orb_class_instance, - ORB_PRIO_DEFAULT); + // State machine will have generated a report, copy it out. + if (_reports->get(read_buffer)) { + buffer_size = sizeof(*read_buffer); } - _reports->force(&report); + return buffer_size; +} + +int +VL53LXX::readRegister(const uint8_t reg_address, uint8_t &value) +{ + // Write register address to the sensor. + int ret = transfer(®_address, sizeof(reg_address), nullptr, 0); - /* notify anyone waiting for data */ - poll_notify(POLLIN); + if (ret != PX4_OK) { + perf_count(_comms_errors); + return ret; + } - ret = OK; + // Read from the sensor. + ret = transfer(nullptr, 0, &value, 1); - perf_end(_sample_perf); + if (ret != PX4_OK) { + perf_count(_comms_errors); + return ret; + } - return ret; + return PX4_OK; } -void -VL53LXX::start() +int +VL53LXX::readRegisterMulti(const uint8_t reg_address, uint8_t *value, const uint8_t length) { - /* reset the report ring and state machine */ - _reports->flush(); + // Write register address to the sensor. + int ret = transfer(®_address, 1, nullptr, 0); - /* schedule a cycle to start things */ - ScheduleDelayed(VL53LXX_US); -} + if (ret != PX4_OK) { + perf_count(_comms_errors); + return ret; + } -void -VL53LXX::stop() -{ - ScheduleClear(); + // Read from the sensor. + ret = transfer(nullptr, 0, &value[0], length); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + return ret; + } + + return PX4_OK; } void @@ -703,24 +483,14 @@ VL53LXX::Run() } } -void -VL53LXX::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u\n", _measure_interval); - _reports->print_info("report queue"); -} - -bool +int VL53LXX::spadCalculations() { - uint8_t val; - - uint8_t spad_count; - bool spad_type_is_aperture; + uint8_t val = 0; + uint8_t spad_count = 0; + uint8_t ref_spad_map[6] = {}; - uint8_t ref_spad_map[6]; + bool spad_type_is_aperture = false; writeRegister(0x80, 0x01); writeRegister(0xFF, 0x01); @@ -743,7 +513,6 @@ VL53LXX::spadCalculations() } writeRegister(0x83, 0x01); - readRegister(0x92, val); spad_count = val & 0x7f; @@ -784,32 +553,79 @@ VL53LXX::spadCalculations() sensorTuning(); - writeRegister(SYSTEM_INTERRUPT_CONFIG_GPIO_REG, 4); // 4: GPIO interrupt on new data - + writeRegister(SYSTEM_INTERRUPT_CONFIG_GPIO_REG, 4); // 4: GPIO interrupt on new data. readRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val); - writeRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val & ~0x10); // active low + writeRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val & ~0x10); // Active low. writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); - writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); - writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x01); + singleRefCalibration(0x40); writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x02); + singleRefCalibration(0x00); - writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // restore config + writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // Restore config. return OK; - } +int +VL53LXX::sensorInit() +{ + uint8_t val = 0; + + // I2C at 2.8V on sensor side of level shifter + int ret = PX4_OK; + ret |= readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + ret |= writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01); + + // set I2C to standard mode + ret |= writeRegister(0x88, 0x00); + ret |= writeRegister(0x80, 0x01); + ret |= writeRegister(0xFF, 0x01); + ret |= writeRegister(0x00, 0x00); + ret |= readRegister(0x91, val); + ret |= writeRegister(0x00, 0x01); + ret |= writeRegister(0xFF, 0x00); + ret |= writeRegister(0x80, 0x00); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + _stop_variable = val; + + // Disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks + readRegister(MSRC_CONFIG_CONTROL_REG, val); + writeRegister(MSRC_CONFIG_CONTROL_REG, val | 0x12); + + // Set signal rate limit to 0.1 + float rate_limit = 0.1 * 65536; + uint8_t rate_limit_split[2] = {}; + + rate_limit_split[0] = (((uint16_t)rate_limit) >> 8); + rate_limit_split[1] = (uint16_t)rate_limit; + + writeRegisterMulti(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG, &rate_limit_split[0], 2); + writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xFF); + + spadCalculations(); + + return PX4_OK; +} -bool +int VL53LXX::sensorTuning() { - + // Magic register settings taken from the ST Micro API. writeRegister(0xFF, 0x01); writeRegister(0x00, 0x00); writeRegister(0xFF, 0x00); @@ -891,28 +707,86 @@ VL53LXX::sensorTuning() writeRegister(0xFF, 0x00); writeRegister(0x80, 0x00); - return OK; + return PX4_OK; } - -bool -VL53LXX::singleRefCalibration(uint8_t byte) +int +VL53LXX::singleRefCalibration(const uint8_t byte) { - uint8_t val; + uint8_t val = 0; - writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP + writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP + readRegister(RESULT_INTERRUPT_STATUS_REG, val); - do { + while ((val & 0x07) == 0) { readRegister(RESULT_INTERRUPT_STATUS_REG, val); - } while ((val & 0x07) == 0); + } writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); writeRegister(SYSRANGE_START_REG, 0x00); - return OK; + return PX4_OK; +} + +void +VL53LXX::start() +{ + // Flush the report ring buffer. + _reports->flush(); + + // Schedule the first cycle. + ScheduleNow(); +} + +void +VL53LXX::stop() +{ + ScheduleClear(); +} + +int +VL53LXX::writeRegister(const uint8_t reg_address, const uint8_t value) +{ + uint8_t cmd[2] = {}; + cmd[0] = reg_address; + cmd[1] = value; + + int ret = transfer(&cmd[0], 2, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + return ret; + } + + return PX4_OK; } +int +VL53LXX::writeRegisterMulti(const uint8_t reg_address, const uint8_t *value, + const uint8_t length) /* bytes are send in order as they are in the array */ +{ + if (length > 6 || length < 1) { + DEVICE_LOG("VL53LXX::writeRegisterMulti length out of range"); + return PX4_ERROR; + } + + /* be careful: for uint16_t to send first higher byte */ + uint8_t cmd[7] = {}; + cmd[0] = reg_address; + + memcpy(&cmd[1], &value[0], length); + + int ret = transfer(&cmd[0], length + 1, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + return ret; + } + + return PX4_OK; +} + /** * Local functions in support of the shell command. @@ -920,32 +794,44 @@ VL53LXX::singleRefCalibration(uint8_t byte) namespace vl53lxx { -VL53LXX *g_dev; +VL53LXX *g_dev; -int start(uint8_t rotation); -int start_bus(uint8_t rotation, int i2c_bus); -int stop(); -int test(); -int info(); +int reset(); +int start(const uint8_t rotation); +int start_bus(const uint8_t rotation = 0, const int i2c_bus = VL53LXX_BUS_DEFAULT); +int status(); +int stop(); +int test(); +int usage(); + +/** + * Reset the driver. + */ +int +reset() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; + } + + g_dev->stop(); + g_dev->start(); + PX4_INFO("driver reset"); + return PX4_OK; +} /** - * * Attempt to start driver on all available I2C busses. * * This function will return as soon as the first sensor * is detected on one of the available busses or if no * sensors are detected. - * */ int -start(uint8_t rotation) +start(const uint8_t rotation) { - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_ERROR; - } - - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { + for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) { return PX4_OK; } @@ -961,52 +847,48 @@ start(uint8_t rotation) * or could not be detected successfully. */ int -start_bus(uint8_t rotation, int i2c_bus) +start_bus(const uint8_t rotation, const int i2c_bus) { - int fd = -1; - if (g_dev != nullptr) { PX4_ERR("already started"); - return PX4_ERROR; + return PX4_OK; } - /* create the driver */ + // Instantiate the driver. g_dev = new VL53LXX(rotation, i2c_bus); if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(VL53LXX_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; + delete g_dev; + return PX4_ERROR; } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; + // Initialize the sensor. + if (g_dev->init() != PX4_OK) { + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; } - px4_close(fd); + // Start the driver. + g_dev->start(); + PX4_INFO("driver started"); return PX4_OK; +} -fail: - - if (fd >= 0) { - px4_close(fd); +/** + * Print the driver status. + */ +int +status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; } - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } + g_dev->print_info(); - return PX4_ERROR; + return PX4_OK; } /** @@ -1019,11 +901,9 @@ stop() delete g_dev; g_dev = nullptr; - } else { - PX4_ERR("driver not running"); - return PX4_ERROR; } + PX4_INFO("driver stopped"); return PX4_OK; } @@ -1035,9 +915,6 @@ stop() int test() { - struct distance_sensor_s report; - ssize_t sz; - int fd = px4_open(VL53LXX_DEVICE_PATH, O_RDONLY); if (fd < 0) { @@ -1045,10 +922,11 @@ test() return PX4_ERROR; } - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); + // Perform a simple demand read. + distance_sensor_s report {}; + ssize_t num_bytes = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) { + if (num_bytes != sizeof(report)) { PX4_ERR("immediate read failed"); return PX4_ERROR; } @@ -1061,74 +939,67 @@ test() return PX4_OK; } - -/** - * Print a little info about the driver. - */ int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return PX4_OK; -} - -} // namespace vl53lxx - - -static void -vl53lxx_usage() +usage() { PX4_INFO("usage: vl53lxx command [options]"); PX4_INFO("options:"); + PX4_INFO("\t-a --all available busses"); PX4_INFO("\t-b --bus i2cbus (%d)", VL53LXX_BUS_DEFAULT); - PX4_INFO("\t-a --all"); PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING); PX4_INFO("command:"); - PX4_INFO("\tstart|stop|test|info"); + PX4_INFO("\treset|start|status|stop|test|usage"); + return PX4_OK; } +} // namespace vl53lxx -int -vl53lxx_main(int argc, char *argv[]) + +/** + * Driver 'main' command. + */ +extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[]) { - int ch; - int myoptind = 1; const char *myoptarg = nullptr; - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - bool start_all = false; + int ch = 0; int i2c_bus = VL53LXX_BUS_DEFAULT; + int myoptind = 1; + + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + + bool start_all = false; while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); + case 'a': + start_all = true; break; case 'b': i2c_bus = atoi(myoptarg); break; - case 'a': - start_all = true; + case 'R': + rotation = (uint8_t)atoi(myoptarg); break; default: PX4_WARN("Unknown option!"); - goto out_error; + return vl53lxx::usage(); } } - /* - * Start/load the driver. - */ + if (myoptind >= argc) { + return vl53lxx::usage(); + } + + // Reset the driver. + if (!strcmp(argv[myoptind], "reset")) { + return vl53lxx::reset(); + } + + // Start/load the driver. if (!strcmp(argv[myoptind], "start")) { if (start_all) { return vl53lxx::start(rotation); @@ -1138,29 +1009,21 @@ vl53lxx_main(int argc, char *argv[]) } } - /* - * Stop the driver - */ + // Print the driver status. + if (!strcmp(argv[myoptind], "status")) { + return vl53lxx::status(); + } + + // Stop the driver. if (!strcmp(argv[myoptind], "stop")) { return vl53lxx::stop(); } - /* - * Test the driver/device. - */ + // Test the driver/device. if (!strcmp(argv[myoptind], "test")) { return vl53lxx::test(); } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - return vl53lxx::info(); - } - -out_error: - - vl53lxx_usage(); - return PX4_ERROR; + // Print driver usage information. + return vl53lxx::usage(); }