From ddf75db154ac4f91b462694514302d03ffda24bd Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Fri, 9 Mar 2018 09:26:14 +0100 Subject: [PATCH] vl53lxx driver: cleanup for pull request --- .../distance_sensor/vl53lxx/vl53lxx.cpp | 699 +++++++++--------- 1 file changed, 346 insertions(+), 353 deletions(-) diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 36553a2d53..9b22084626 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -73,13 +73,13 @@ /* Configuration Constants */ #ifdef PX4_I2C_BUS_EXPANSION3 -#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION3 // I2C port (I2C4) on fmu-v5 +#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION3 // I2C port (I2C4) on fmu-v5 #else -#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION // I2C on all others (pixracer) +#define VL53LXX_BUS PX4_I2C_BUS_EXPANSION // I2C on all others (pixracer) #endif -#define VL53LXX_BASEADDR 0b0101001 /* 7-bit address */ -#define VL53LXX_DEVICE_PATH "/dev/vl53lxx" +#define VL53LXX_BASEADDR 0b0101001 // 7-bit address +#define VL53LXX_DEVICE_PATH "/dev/vl53lxx" /* VL53LXX Registers addresses */ #define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 @@ -100,7 +100,11 @@ #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B #define RESULT_RANGE_STATUS_REG 0x14 -#define VL53LXX_CONVERSION_INTERVAL 1000 /* 1ms */ +#define VL53LXX_CONVERSION_INTERVAL 1000 /* 1ms */ +#define VL53LXX_SAMPLE_RATE 20000 /* 20ms */ + +#define VL53LXX_MAX_RANGING_DISTANCE 2.0f +#define VL53LXX_MIN_RANGING_DISTANCE 0.0f #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -111,39 +115,36 @@ class VL53LXX : public device::I2C public: VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = VL53LXX_BUS, int address = VL53LXX_BASEADDR); + virtual ~VL53LXX(); - virtual int init(); + virtual int init(); - //virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); // NOT TESTED YET + + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); private: - uint8_t _rotation; - float _min_distance; - float _max_distance; - work_s _work; + uint8_t _rotation; + work_s _work; ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - uint8_t _valid; - int _measure_ticks; - bool _collect_phase; - int _class_instance; - int _orb_class_instance; - - orb_advert_t _distance_sensor_topic; + bool _sensor_ok; + int _measure_ticks; + int _class_instance; + int _orb_class_instance; + bool _collect_phase; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + orb_advert_t _distance_sensor_topic; - uint8_t _multi_read_value[6]; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; - uint8_t stop_variable_; + uint8_t stop_variable_; /** @@ -152,31 +153,31 @@ private: * @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(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement * and start a new one. - */ - void cycle(); - int measure(); - int collect(); + */ + void cycle(); + int measure(); + int collect(); + + int readRegister(uint8_t reg_address, uint8_t &value); + int writeRegister(uint8_t reg_address, uint8_t value); - int readRegister(uint8_t reg_address, uint8_t &value); - int writeRegister(uint8_t reg_address, uint8_t value); - int writeRegisterMulti(uint8_t reg_address, uint8_t* value, uint8_t length); - //int readRegisterMulti(uint8_t reg_address, uint8_t length); - int readRegisterMulti(uint8_t reg_address, uint8_t *value, 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 sensorInit(); - bool spadCalculations(); - bool sensorTuning(); - bool singleRefCalibration(uint8_t byte); + int sensorInit(); + bool spadCalculations(); + bool sensorTuning(); + bool singleRefCalibration(uint8_t byte); /** * Static trampoline from the workq context; because we don't have a @@ -184,7 +185,7 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; @@ -198,16 +199,13 @@ 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, 100000), _rotation(rotation), - _min_distance(-1.0f), - _max_distance(-1.0f), _reports(nullptr), _sensor_ok(false), - _valid(0), _measure_ticks(0), - _collect_phase(false), _class_instance(-1), _orb_class_instance(-1), _distance_sensor_topic(nullptr), + _collect_phase(false), _sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")), _comms_errors(perf_alloc(PC_COUNT, "tr1_com_err")) { @@ -271,9 +269,9 @@ VL53LXX::sensorInit() writeRegister(MSRC_CONFIG_CONTROL_REG, val | 0x12); // Set signal rate limit to 0.1 - rate_limit = 0.1 * (65536); + rate_limit = 0.1 * 65536; rate_limit_split[0] = (((uint16_t)rate_limit) >> 8); - rate_limit_split[1] = ((uint16_t)rate_limit); + 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); @@ -312,7 +310,7 @@ VL53LXX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* 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, @@ -335,32 +333,41 @@ out: int VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - printf("ioctl\n"); 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_ticks == 0); + bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ _measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - // if (want_start) { - // start(); - // } + if (want_start) { + start(); + } start(); return OK; } + case SENSOR_POLLRATE_MANUAL: { + + stop(); + _measure_ticks = 0; + return OK; + } + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ - //bool want_start = (_measure_ticks == 0); + bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ unsigned ticks = USEC2TICK(1000000 / arg); @@ -369,9 +376,9 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - //if (want_start) { + if (want_start) { start(); - //} + } return OK; } @@ -384,65 +391,63 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long 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_ticks > 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 */ -// usleep(VL53LXX_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; -// } + +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_ticks > 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; + } + + /* 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 @@ -450,15 +455,17 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value) { int ret; uint8_t val = 0; + ret = transfer(®_address, sizeof(reg_address), nullptr, 0); if (OK != ret) { DEVICE_LOG("i2c::transfer returned %d", ret); + perf_count(_comms_errors); return ret; } /* wait for it to complete */ - //usleep(VL53LXX_CONVERSION_INTERVAL); // maybe not needed ??? + usleep(VL53LXX_CONVERSION_INTERVAL); /* read from the sensor */ ret = transfer(nullptr, 0, &val, 1); @@ -466,7 +473,6 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value) if (ret < 0) { DEVICE_LOG("error reading from sensor: %d", ret); perf_count(_comms_errors); - perf_end(_sample_perf); return ret; } @@ -483,7 +489,7 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) { int ret; uint8_t val[6] = {0, 0, 0, 0, 0, 0}; - ret = transfer(®_address, sizeof(reg_address), nullptr, 0); + ret = transfer(®_address, 1, nullptr, 0); if (OK != ret) { DEVICE_LOG("i2c::transfer returned %d", ret); @@ -491,7 +497,7 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) } /* wait for it to complete */ - usleep(VL53LXX_CONVERSION_INTERVAL); // maybe not needed ??? + usleep(VL53LXX_CONVERSION_INTERVAL); /* read from the sensor */ ret = transfer(nullptr, 0, &val[0], length); @@ -499,16 +505,10 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length) if (ret < 0) { DEVICE_LOG("error reading from sensor: %d", ret); perf_count(_comms_errors); - perf_end(_sample_perf); return ret; } - - for (int i = 0; i < length; ++i) // use memcpy - { - value[i] = val[i]; - printf("_multi_read_value[%d]: %u\n", i, (uint16_t)_multi_read_value[i] ); - } + memcpy(&value[0], &val[0], length); ret = OK; @@ -529,6 +529,7 @@ VL53LXX::writeRegister(uint8_t reg_address, uint8_t value) ret = transfer(&cmd[0], 2, nullptr, 0); if (OK != ret) { + perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } @@ -548,15 +549,12 @@ VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t* value, uint8_t length) cmd[0] = reg_address; - for (int i = 0; i < length; i++) //use memcpy - { - cmd[i+1] = value[i]; - } - + memcpy(&cmd[1], &value[0], length); ret = transfer(&cmd[0], length+1, nullptr, 0); if (OK != ret) { + perf_count(_comms_errors); DEVICE_LOG("i2c::transfer returned %d", ret); return ret; } @@ -580,29 +578,43 @@ VL53LXX::measure() */ const uint8_t cmd = RESULT_RANGE_STATUS_REG + 10; - writeRegister(0x80, 0x01); - writeRegister(0xFF, 0x01); - writeRegister(0x00, 0x00); - writeRegister(0x91, stop_variable_); - writeRegister(0x00, 0x01); - writeRegister(0xFF, 0x00); - writeRegister(0x80, 0x00); + if(_new_measurement) { + + _new_measurement = false; - writeRegister(SYSRANGE_START_REG, 0x01); + writeRegister(0x80, 0x01); + writeRegister(0xFF, 0x01); + writeRegister(0x00, 0x00); + writeRegister(0x91, stop_variable_); + writeRegister(0x00, 0x01); + writeRegister(0xFF, 0x00); + writeRegister(0x80, 0x00); + writeRegister(SYSRANGE_START_REG, 0x01); // maybe could be removed by putting sensor + // in continuous mode - readRegister(SYSRANGE_START_REG, system_start); - while((system_start & 0x01) == 1){ readRegister(SYSRANGE_START_REG, system_start); - } + while((system_start & 0x01) == 1) { + readRegister(SYSRANGE_START_REG, system_start); + usleep(1000); + } + } readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); - while((wait_for_measurement & 0x07) == 0){ - readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); + + if((wait_for_measurement & 0x07) == 0){ + work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000); + return; } + _collect_phase = true; + + // while((wait_for_measurement & 0x07) == 0) { + // readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement); + // usleep(1000); + // } ret = transfer(&cmd, sizeof(cmd), nullptr, 0); @@ -645,9 +657,10 @@ VL53LXX::collect() report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.orientation = _rotation; report.current_distance = distance_m; - report.min_distance = 0.0f; - report.max_distance = 2.0f; + report.min_distance = VL53LXX_MIN_RANGING_DISTANCE; + report.max_distance = VL53LXX_MAX_RANGING_DISTANCE; report.covariance = 0.0f; + /* TODO: set proper ID */ report.id = 0; @@ -667,11 +680,11 @@ VL53LXX::collect() return ret; } + void VL53LXX::start() { /* reset the report ring and state machine */ - _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ @@ -687,19 +700,22 @@ VL53LXX::start() static orb_advert_t pub = nullptr; if (pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); } } + void VL53LXX::stop() { work_cancel(HPWORK, &_work); } + void VL53LXX::cycle_trampoline(void *arg) { @@ -708,21 +724,26 @@ VL53LXX::cycle_trampoline(void *arg) dev->cycle(); } + void VL53LXX::cycle() { - measure(); - collect(); + measure(); // no wait state between measure() and collect() + if(_collect_phase) { + + _collect_phase = false; + collect(); - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, + work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, - USEC2TICK(10000)); + USEC2TICK(VL53LXX_SAMPLE_RATE)); + } } + void VL53LXX::print_info() { @@ -736,96 +757,96 @@ VL53LXX::print_info() bool VL53LXX::spadCalculations() { - uint8_t val; + uint8_t val; - uint8_t spad_count; - bool spad_type_is_aperture; + uint8_t spad_count; + bool spad_type_is_aperture; - uint8_t ref_spad_map[6]; + uint8_t ref_spad_map[6]; - writeRegister(0x80, 0x01); - writeRegister(0xFF, 0x01); - writeRegister(0x00, 0x00); + writeRegister(0x80, 0x01); + writeRegister(0xFF, 0x01); + writeRegister(0x00, 0x00); + writeRegister(0xFF, 0x06); - writeRegister(0xFF, 0x06); + readRegister(0x83, val); + writeRegister(0x83, val | 0x04); - readRegister(0x83, val); - writeRegister(0x83, val | 0x04); - writeRegister(0xFF, 0x07); - writeRegister(0x81, 0x01); + writeRegister(0xFF, 0x07); + writeRegister(0x81, 0x01); + writeRegister(0x80, 0x01); + writeRegister(0x94, 0x6b); + writeRegister(0x83, 0x00); - writeRegister(0x80, 0x01); + readRegister(0x83, val); - writeRegister(0x94, 0x6b); - writeRegister(0x83, 0x00); + while (val == 0x00) { + readRegister(0x83, val); + } - readRegister(0x83, val); + writeRegister(0x83, 0x01); - while (val == 0x00){ - readRegister(0x83, val); - } + readRegister(0x92, val); - writeRegister(0x83, 0x01); - readRegister(0x92, val); + spad_count = val & 0x7f; + spad_type_is_aperture = (val >> 7) & 0x01; - spad_count = val & 0x7f; - spad_type_is_aperture = (val >> 7) & 0x01; + writeRegister(0x81, 0x00); + writeRegister(0xFF, 0x06); - writeRegister(0x81, 0x00); - writeRegister(0xFF, 0x06); - readRegister(0x83, val); - writeRegister(0x83, val & ~0x04); - writeRegister(0xFF, 0x01); - writeRegister(0x00, 0x01); + readRegister(0x83, val); + writeRegister(0x83, val & ~0x04); - writeRegister(0xFF, 0x00); - writeRegister(0x80, 0x00); - - readRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6); - - writeRegister(0xFF, 0x01); - writeRegister(DYNAMIC_SPAD_REF_EN_START_OFFSET_REG, 0x00); - writeRegister(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG, 0x2C); - writeRegister(0xFF, 0x00); - writeRegister(GLOBAL_CONFIG_REF_EN_START_SELECT_REG, 0xB4); - - uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; - uint8_t spads_enabled = 0; - - for (uint8_t i = 0; i < 48; i++) - { - if (i < first_spad_to_enable || spads_enabled == spad_count) - { - ref_spad_map[i / 8] &= ~(1 << (i % 8)); - } - else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1) - { - spads_enabled++; - } - } - - writeRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6); - - sensorTuning(); - - 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(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); + writeRegister(0xFF, 0x01); + writeRegister(0x00, 0x01); + writeRegister(0xFF, 0x00); + writeRegister(0x80, 0x00); - writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); + readRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6); + + writeRegister(0xFF, 0x01); + writeRegister(DYNAMIC_SPAD_REF_EN_START_OFFSET_REG, 0x00); + writeRegister(DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD_REG, 0x2C); + writeRegister(0xFF, 0x00); + writeRegister(GLOBAL_CONFIG_REF_EN_START_SELECT_REG, 0xB4); + + uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0; + uint8_t spads_enabled = 0; + + for (uint8_t i = 0; i < 48; i++) + { + if (i < first_spad_to_enable || spads_enabled == spad_count) + { + ref_spad_map[i / 8] &= ~(1 << (i % 8)); + } + else if ((ref_spad_map[i / 8] >> (i % 8)) & 0x1) + { + spads_enabled++; + } + } + + writeRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6); + + sensorTuning(); + + 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(SYSTEM_SEQUENCE_CONFIG_REG, 0x01); - singleRefCalibration(0x40); + writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); - writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x02); - singleRefCalibration(0x00); + writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); - writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // restore config + writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x01); + singleRefCalibration(0x40); + + writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x02); + singleRefCalibration(0x00); + writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // restore config - return true; + return true; } @@ -834,122 +855,108 @@ bool VL53LXX::sensorTuning() { - writeRegister(0xFF, 0x01); - writeRegister(0x00, 0x00); - - writeRegister(0xFF, 0x00); - writeRegister(0x09, 0x00); - writeRegister(0x10, 0x00); - writeRegister(0x11, 0x00); - - writeRegister(0x24, 0x01); - writeRegister(0x25, 0xFF); - writeRegister(0x75, 0x00); - - writeRegister(0xFF, 0x01); - writeRegister(0x4E, 0x2C); - writeRegister(0x48, 0x00); - writeRegister(0x30, 0x20); - - writeRegister(0xFF, 0x00); - writeRegister(0x30, 0x09); - writeRegister(0x54, 0x00); - writeRegister(0x31, 0x04); - writeRegister(0x32, 0x03); - writeRegister(0x40, 0x83); - writeRegister(0x46, 0x25); - writeRegister(0x60, 0x00); - writeRegister(0x27, 0x00); - writeRegister(0x50, 0x06); - writeRegister(0x51, 0x00); - writeRegister(0x52, 0x96); - writeRegister(0x56, 0x08); - writeRegister(0x57, 0x30); - writeRegister(0x61, 0x00); - writeRegister(0x62, 0x00); - writeRegister(0x64, 0x00); - writeRegister(0x65, 0x00); - writeRegister(0x66, 0xA0); - - writeRegister(0xFF, 0x01); - writeRegister(0x22, 0x32); - writeRegister(0x47, 0x14); - writeRegister(0x49, 0xFF); - writeRegister(0x4A, 0x00); - - writeRegister(0xFF, 0x00); - writeRegister(0x7A, 0x0A); - writeRegister(0x7B, 0x00); - writeRegister(0x78, 0x21); - - writeRegister(0xFF, 0x01); - writeRegister(0x23, 0x34); - writeRegister(0x42, 0x00); - writeRegister(0x44, 0xFF); - writeRegister(0x45, 0x26); - writeRegister(0x46, 0x05); - writeRegister(0x40, 0x40); - writeRegister(0x0E, 0x06); - writeRegister(0x20, 0x1A); - writeRegister(0x43, 0x40); - - writeRegister(0xFF, 0x00); - writeRegister(0x34, 0x03); - writeRegister(0x35, 0x44); - - writeRegister(0xFF, 0x01); - writeRegister(0x31, 0x04); - writeRegister(0x4B, 0x09); - writeRegister(0x4C, 0x05); - writeRegister(0x4D, 0x04); - - writeRegister(0xFF, 0x00); - writeRegister(0x44, 0x00); - writeRegister(0x45, 0x20); - writeRegister(0x47, 0x08); - writeRegister(0x48, 0x28); - writeRegister(0x67, 0x00); - writeRegister(0x70, 0x04); - writeRegister(0x71, 0x01); - writeRegister(0x72, 0xFE); - writeRegister(0x76, 0x00); - writeRegister(0x77, 0x00); - - writeRegister(0xFF, 0x01); - writeRegister(0x0D, 0x01); - - writeRegister(0xFF, 0x00); - writeRegister(0x80, 0x01); - writeRegister(0x01, 0xF8); - - writeRegister(0xFF, 0x01); - writeRegister(0x8E, 0x01); - writeRegister(0x00, 0x01); - writeRegister(0xFF, 0x00); - writeRegister(0x80, 0x00); - - return true; + writeRegister(0xFF, 0x01); + writeRegister(0x00, 0x00); + writeRegister(0xFF, 0x00); + writeRegister(0x09, 0x00); + writeRegister(0x10, 0x00); + writeRegister(0x11, 0x00); + writeRegister(0x24, 0x01); + writeRegister(0x25, 0xFF); + writeRegister(0x75, 0x00); + writeRegister(0xFF, 0x01); + writeRegister(0x4E, 0x2C); + writeRegister(0x48, 0x00); + writeRegister(0x30, 0x20); + writeRegister(0xFF, 0x00); + writeRegister(0x30, 0x09); + writeRegister(0x54, 0x00); + writeRegister(0x31, 0x04); + writeRegister(0x32, 0x03); + writeRegister(0x40, 0x83); + writeRegister(0x46, 0x25); + writeRegister(0x60, 0x00); + writeRegister(0x27, 0x00); + writeRegister(0x50, 0x06); + writeRegister(0x51, 0x00); + writeRegister(0x52, 0x96); + writeRegister(0x56, 0x08); + writeRegister(0x57, 0x30); + writeRegister(0x61, 0x00); + writeRegister(0x62, 0x00); + writeRegister(0x64, 0x00); + writeRegister(0x65, 0x00); + writeRegister(0x66, 0xA0); + writeRegister(0xFF, 0x01); + writeRegister(0x22, 0x32); + writeRegister(0x47, 0x14); + writeRegister(0x49, 0xFF); + writeRegister(0x4A, 0x00); + writeRegister(0xFF, 0x00); + writeRegister(0x7A, 0x0A); + writeRegister(0x7B, 0x00); + writeRegister(0x78, 0x21); + writeRegister(0xFF, 0x01); + writeRegister(0x23, 0x34); + writeRegister(0x42, 0x00); + writeRegister(0x44, 0xFF); + writeRegister(0x45, 0x26); + writeRegister(0x46, 0x05); + writeRegister(0x40, 0x40); + writeRegister(0x0E, 0x06); + writeRegister(0x20, 0x1A); + writeRegister(0x43, 0x40); + writeRegister(0xFF, 0x00); + writeRegister(0x34, 0x03); + writeRegister(0x35, 0x44); + writeRegister(0xFF, 0x01); + writeRegister(0x31, 0x04); + writeRegister(0x4B, 0x09); + writeRegister(0x4C, 0x05); + writeRegister(0x4D, 0x04); + writeRegister(0xFF, 0x00); + writeRegister(0x44, 0x00); + writeRegister(0x45, 0x20); + writeRegister(0x47, 0x08); + writeRegister(0x48, 0x28); + writeRegister(0x67, 0x00); + writeRegister(0x70, 0x04); + writeRegister(0x71, 0x01); + writeRegister(0x72, 0xFE); + writeRegister(0x76, 0x00); + writeRegister(0x77, 0x00); + writeRegister(0xFF, 0x01); + writeRegister(0x0D, 0x01); + writeRegister(0xFF, 0x00); + writeRegister(0x80, 0x01); + writeRegister(0x01, 0xF8); + writeRegister(0xFF, 0x01); + writeRegister(0x8E, 0x01); + writeRegister(0x00, 0x01); + writeRegister(0xFF, 0x00); + writeRegister(0x80, 0x00); + + return true; } bool VL53LXX::singleRefCalibration(uint8_t byte) { - uint8_t val; - - writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP + uint8_t val; - do - { - readRegister(RESULT_INTERRUPT_STATUS_REG, val); - } - while ((val & 0x07) == 0); + writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP - writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); + do + { + readRegister(RESULT_INTERRUPT_STATUS_REG, val); + } + while ((val & 0x07) == 0); - writeRegister(SYSRANGE_START_REG, 0x00); + writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); + writeRegister(SYSRANGE_START_REG, 0x00); - return true; + return true; } @@ -1039,25 +1046,7 @@ void stop() void test() { - struct distance_sensor_s report; - - if (g_dev != nullptr) { - errx(1, "already started"); - } - - /* create the driver */ - g_dev = new VL53LXX(0, VL53LXX_BUS); - - if (g_dev == nullptr) { - delete g_dev; - g_dev = nullptr; - } - - if (OK != g_dev->init()) { - delete g_dev; - g_dev = nullptr; - - } + //struct distance_sensor_s report; int fd = open(VL53LXX_DEVICE_PATH, O_RDONLY); @@ -1065,9 +1054,6 @@ test() err(1, "%s open failed (try 'vl53lxx start' if the driver is not running", VL53LXX_DEVICE_PATH); } - warnx("single read"); - warnx("measurement: %0.2f m", (double)report.current_distance); - warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { @@ -1116,7 +1102,8 @@ info() exit(0); } -} // namespace +} // namespace vl53lxx + int vl53lxx_main(int argc, char *argv[]) @@ -1126,6 +1113,10 @@ vl53lxx_main(int argc, char *argv[]) const char *myoptarg = nullptr; uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + if (argc < 2) { + goto out; + } + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': @@ -1173,6 +1164,8 @@ vl53lxx_main(int argc, char *argv[]) vl53lxx::info(); } +out: + PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); return PX4_ERROR; }