fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 09:11:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

vl53lxx driver: cleanup for pull request

Browse Source
This commit is contained in:
DanielePettenuzzo
2018-05-22 12:21:45 +02:00
committed by Beat Küng
parent c73ca29f44
commit ddf75db154
1 changed files with 333 additions and 340 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
+330 -337
View File
@@ -73,13 +73,13 @@
/* Configuration Constants */ /* Configuration Constants */
#ifdef PX4_I2C_BUS_EXPANSION3 #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 #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 #endif
#define VL53LXX_BASEADDR 0b0101001 /* 7-bit address */ #define VL53LXX_BASEADDR 0b0101001 // 7-bit address
#define VL53LXX_DEVICE_PATH "/dev/vl53lxx" #define VL53LXX_DEVICE_PATH "/dev/vl53lxx"
/* VL53LXX Registers addresses */ /* VL53LXX Registers addresses */
#define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89 #define VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG 0x89
@@ -100,7 +100,11 @@
#define SYSTEM_INTERRUPT_CLEAR_REG 0x0B #define SYSTEM_INTERRUPT_CLEAR_REG 0x0B
#define RESULT_RANGE_STATUS_REG 0x14 #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 #ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE. # error This requires CONFIG_SCHED_WORKQUEUE.
@@ -111,39 +115,36 @@ class VL53LXX : public device::I2C
public: public:
VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
int bus = VL53LXX_BUS, int address = VL53LXX_BASEADDR); int bus = VL53LXX_BUS, int address = VL53LXX_BASEADDR);
virtual ~VL53LXX(); virtual ~VL53LXX();
virtual int init(); virtual int init();
//virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); 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);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
*/ */
void print_info(); void print_info();
private: private:
uint8_t _rotation; uint8_t _rotation;
float _min_distance; work_s _work;
float _max_distance;
work_s _work;
ringbuffer::RingBuffer *_reports; ringbuffer::RingBuffer *_reports;
bool _sensor_ok; bool _sensor_ok;
uint8_t _valid; int _measure_ticks;
int _measure_ticks; int _class_instance;
bool _collect_phase; int _orb_class_instance;
int _class_instance; bool _collect_phase;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic; orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
uint8_t _multi_read_value[6]; 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 * @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. * to make it more aggressive about resetting the bus in case of errors.
*/ */
void start(); void start();
/** /**
* Stop the automatic measurement state machine. * Stop the automatic measurement state machine.
*/ */
void stop(); void stop();
/** /**
* Perform a poll cycle; collect from the previous measurement * Perform a poll cycle; collect from the previous measurement
* and start a new one. * and start a new one.
*/ */
void cycle(); void cycle();
int measure(); int measure();
int collect(); int collect();
int readRegister(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 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 sensorInit(); int writeRegisterMulti(uint8_t reg_address, uint8_t* value, uint8_t length);
bool spadCalculations(); int readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length);
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 * 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. * @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) : VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 100000), I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 100000),
_rotation(rotation), _rotation(rotation),
_min_distance(-1.0f),
_max_distance(-1.0f),
_reports(nullptr), _reports(nullptr),
_sensor_ok(false), _sensor_ok(false),
_valid(0),
_measure_ticks(0), _measure_ticks(0),
_collect_phase(false),
_class_instance(-1), _class_instance(-1),
_orb_class_instance(-1), _orb_class_instance(-1),
_distance_sensor_topic(nullptr), _distance_sensor_topic(nullptr),
_collect_phase(false),
_sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")), _sample_perf(perf_alloc(PC_ELAPSED, "tr1_read")),
_comms_errors(perf_alloc(PC_COUNT, "tr1_com_err")) _comms_errors(perf_alloc(PC_COUNT, "tr1_com_err"))
{ {
@@ -271,9 +269,9 @@ VL53LXX::sensorInit()
writeRegister(MSRC_CONFIG_CONTROL_REG, val | 0x12); writeRegister(MSRC_CONFIG_CONTROL_REG, val | 0x12);
// Set signal rate limit to 0.1 // 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[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); writeRegisterMulti(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT_REG, &rate_limit_split[0], 2);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xFF); writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xFF);
@@ -312,7 +310,7 @@ VL53LXX::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) { if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */ /* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report; struct distance_sensor_s ds_report;
//measure();
_reports->get(&ds_report); _reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
@@ -335,32 +333,41 @@ out:
int int
VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg) VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{ {
printf("ioctl\n");
switch (cmd) { switch (cmd) {
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (arg) { switch (arg) {
case 0:
return -EINVAL;
case SENSOR_POLLRATE_DEFAULT: { case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */ /* 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 */ /* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL); _measure_ticks = USEC2TICK(VL53LXX_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
// if (want_start) { if (want_start) {
// start(); start();
// } }
start(); start();
return OK; return OK;
} }
case SENSOR_POLLRATE_MANUAL: {
stop();
_measure_ticks = 0;
return OK;
}
/* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
default: { default: {
/* do we need to start internal polling? */ /* 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 */ /* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg); unsigned ticks = USEC2TICK(1000000 / arg);
@@ -369,9 +376,9 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
_measure_ticks = ticks; _measure_ticks = ticks;
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
//if (want_start) { if (want_start) {
start(); start();
//} }
return OK; 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<struct distance_sensor_s *>(buffer);
// int ret = 0;
// /* buffer must be large enough */ ssize_t
// if (count < 1) { VL53LXX::read(device::file_t *filp, char *buffer, size_t buflen)
// return -ENOSPC; {
// } unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
// /* if automatic measurement is enabled */ /* buffer must be large enough */
// if (_measure_ticks > 0) { if (count < 1) {
return -ENOSPC;
}
// /* /* if automatic measurement is enabled */
// * While there is space in the caller's buffer, and reports, copy them. if (_measure_ticks > 0) {
// * 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; * 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++;
}
}
// /* manual measurement - run one conversion */ /* if there was no data, warn the caller */
// do { return ret ? ret : -EAGAIN;
// _reports->flush(); }
// /* trigger a measurement */ /* manual measurement - run one conversion */
// if (OK != measure()) { do {
// ret = -EIO; _reports->flush();
// break;
// }
// /* wait for it to complete */ /* trigger a measurement */
// usleep(VL53LXX_CONVERSION_INTERVAL); if (OK != measure()) {
ret = -EIO;
break;
}
// /* run the collection phase */ /* run the collection phase */
// if (OK != collect()) { if (OK != collect()) {
// ret = -EIO; ret = -EIO;
// break; break;
// } }
// /* state machine will have generated a report, copy it out */ /* state machine will have generated a report, copy it out */
// if (_reports->get(rbuf)) { if (_reports->get(rbuf)) {
// ret = sizeof(*rbuf); ret = sizeof(*rbuf);
// } }
// } while (0); } while (0);
// return ret; return ret;
// } }
int int
@@ -450,15 +455,17 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
{ {
int ret; int ret;
uint8_t val = 0; uint8_t val = 0;
ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0); ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0);
if (OK != ret) { if (OK != ret) {
DEVICE_LOG("i2c::transfer returned %d", ret); DEVICE_LOG("i2c::transfer returned %d", ret);
perf_count(_comms_errors);
return ret; return ret;
} }
/* wait for it to complete */ /* wait for it to complete */
//usleep(VL53LXX_CONVERSION_INTERVAL); // maybe not needed ??? usleep(VL53LXX_CONVERSION_INTERVAL);
/* read from the sensor */ /* read from the sensor */
ret = transfer(nullptr, 0, &val, 1); ret = transfer(nullptr, 0, &val, 1);
@@ -466,7 +473,6 @@ VL53LXX::readRegister(uint8_t reg_address, uint8_t &value)
if (ret < 0) { if (ret < 0) {
DEVICE_LOG("error reading from sensor: %d", ret); DEVICE_LOG("error reading from sensor: %d", ret);
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf);
return ret; return ret;
} }
@@ -483,7 +489,7 @@ VL53LXX::readRegisterMulti(uint8_t reg_address, uint8_t *value, uint8_t length)
{ {
int ret; int ret;
uint8_t val[6] = {0, 0, 0, 0, 0, 0}; uint8_t val[6] = {0, 0, 0, 0, 0, 0};
ret = transfer(&reg_address, sizeof(reg_address), nullptr, 0); ret = transfer(&reg_address, 1, nullptr, 0);
if (OK != ret) { if (OK != ret) {
DEVICE_LOG("i2c::transfer returned %d", 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 */ /* wait for it to complete */
usleep(VL53LXX_CONVERSION_INTERVAL); // maybe not needed ??? usleep(VL53LXX_CONVERSION_INTERVAL);
/* read from the sensor */ /* read from the sensor */
ret = transfer(nullptr, 0, &val[0], length); 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) { if (ret < 0) {
DEVICE_LOG("error reading from sensor: %d", ret); DEVICE_LOG("error reading from sensor: %d", ret);
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf);
return ret; return ret;
} }
memcpy(&value[0], &val[0], length);
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] );
}
ret = OK; ret = OK;
@@ -529,6 +529,7 @@ VL53LXX::writeRegister(uint8_t reg_address, uint8_t value)
ret = transfer(&cmd[0], 2, nullptr, 0); ret = transfer(&cmd[0], 2, nullptr, 0);
if (OK != ret) { if (OK != ret) {
perf_count(_comms_errors);
DEVICE_LOG("i2c::transfer returned %d", ret); DEVICE_LOG("i2c::transfer returned %d", ret);
return ret; return ret;
} }
@@ -548,15 +549,12 @@ VL53LXX::writeRegisterMulti(uint8_t reg_address, uint8_t* value, uint8_t length)
cmd[0] = reg_address; cmd[0] = reg_address;
for (int i = 0; i < length; i++) //use memcpy memcpy(&cmd[1], &value[0], length);
{
cmd[i+1] = value[i];
}
ret = transfer(&cmd[0], length+1, nullptr, 0); ret = transfer(&cmd[0], length+1, nullptr, 0);
if (OK != ret) { if (OK != ret) {
perf_count(_comms_errors);
DEVICE_LOG("i2c::transfer returned %d", ret); DEVICE_LOG("i2c::transfer returned %d", ret);
return ret; return ret;
} }
@@ -580,29 +578,43 @@ VL53LXX::measure()
*/ */
const uint8_t cmd = RESULT_RANGE_STATUS_REG + 10; const uint8_t cmd = RESULT_RANGE_STATUS_REG + 10;
writeRegister(0x80, 0x01); if(_new_measurement) {
writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x00);
writeRegister(0x91, stop_variable_);
writeRegister(0x00, 0x01);
writeRegister(0xFF, 0x00);
writeRegister(0x80, 0x00);
writeRegister(SYSRANGE_START_REG, 0x01); _new_measurement = false;
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); 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); 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); ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
@@ -645,9 +657,10 @@ VL53LXX::collect()
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation; report.orientation = _rotation;
report.current_distance = distance_m; report.current_distance = distance_m;
report.min_distance = 0.0f; report.min_distance = VL53LXX_MIN_RANGING_DISTANCE;
report.max_distance = 2.0f; report.max_distance = VL53LXX_MAX_RANGING_DISTANCE;
report.covariance = 0.0f; report.covariance = 0.0f;
/* TODO: set proper ID */ /* TODO: set proper ID */
report.id = 0; report.id = 0;
@@ -667,11 +680,11 @@ VL53LXX::collect()
return ret; return ret;
} }
void void
VL53LXX::start() VL53LXX::start()
{ {
/* reset the report ring and state machine */ /* reset the report ring and state machine */
_collect_phase = false;
_reports->flush(); _reports->flush();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
@@ -687,19 +700,22 @@ VL53LXX::start()
static orb_advert_t pub = nullptr; static orb_advert_t pub = nullptr;
if (pub != nullptr) { if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else { } else {
pub = orb_advertise(ORB_ID(subsystem_info), &info); pub = orb_advertise(ORB_ID(subsystem_info), &info);
} }
} }
void void
VL53LXX::stop() VL53LXX::stop()
{ {
work_cancel(HPWORK, &_work); work_cancel(HPWORK, &_work);
} }
void void
VL53LXX::cycle_trampoline(void *arg) VL53LXX::cycle_trampoline(void *arg)
{ {
@@ -708,21 +724,26 @@ VL53LXX::cycle_trampoline(void *arg)
dev->cycle(); dev->cycle();
} }
void void
VL53LXX::cycle() VL53LXX::cycle()
{ {
measure(); measure(); // no wait state between measure() and collect()
collect(); if(_collect_phase) {
/* schedule a fresh cycle call when the measurement is done */ _collect_phase = false;
work_queue(HPWORK, collect();
work_queue(HPWORK,
&_work, &_work,
(worker_t)&VL53LXX::cycle_trampoline, (worker_t)&VL53LXX::cycle_trampoline,
this, this,
USEC2TICK(10000)); USEC2TICK(VL53LXX_SAMPLE_RATE));
}
} }
void void
VL53LXX::print_info() VL53LXX::print_info()
{ {
@@ -736,96 +757,96 @@ VL53LXX::print_info()
bool bool
VL53LXX::spadCalculations() VL53LXX::spadCalculations()
{ {
uint8_t val; uint8_t val;
uint8_t spad_count; uint8_t spad_count;
bool spad_type_is_aperture; bool spad_type_is_aperture;
uint8_t ref_spad_map[6]; uint8_t ref_spad_map[6];
writeRegister(0x80, 0x01); writeRegister(0x80, 0x01);
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x00); writeRegister(0x00, 0x00);
writeRegister(0xFF, 0x06);
writeRegister(0xFF, 0x06);
readRegister(0x83, val);
writeRegister(0x83, val | 0x04);
writeRegister(0xFF, 0x07);
writeRegister(0x81, 0x01);
writeRegister(0x80, 0x01);
writeRegister(0x94, 0x6b);
writeRegister(0x83, 0x00);
readRegister(0x83, val);
while (val == 0x00){
readRegister(0x83, val); readRegister(0x83, val);
} writeRegister(0x83, val | 0x04);
writeRegister(0x83, 0x01); writeRegister(0xFF, 0x07);
readRegister(0x92, val); writeRegister(0x81, 0x01);
writeRegister(0x80, 0x01);
writeRegister(0x94, 0x6b);
writeRegister(0x83, 0x00);
spad_count = val & 0x7f; readRegister(0x83, val);
spad_type_is_aperture = (val >> 7) & 0x01;
writeRegister(0x81, 0x00); while (val == 0x00) {
writeRegister(0xFF, 0x06); readRegister(0x83, val);
readRegister(0x83, val); }
writeRegister(0x83, val & ~0x04);
writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x01);
writeRegister(0xFF, 0x00); writeRegister(0x83, 0x01);
writeRegister(0x80, 0x00);
readRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6); readRegister(0x92, val);
writeRegister(0xFF, 0x01); spad_count = val & 0x7f;
writeRegister(DYNAMIC_SPAD_REF_EN_START_OFFSET_REG, 0x00); spad_type_is_aperture = (val >> 7) & 0x01;
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; writeRegister(0x81, 0x00);
uint8_t spads_enabled = 0; writeRegister(0xFF, 0x06);
for (uint8_t i = 0; i < 48; i++) readRegister(0x83, val);
{ writeRegister(0x83, val & ~0x04);
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); writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x01);
writeRegister(0xFF, 0x00);
writeRegister(0x80, 0x00);
sensorTuning(); readRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6);
writeRegister(SYSTEM_INTERRUPT_CONFIG_GPIO_REG, 4); // 4: GPIO interrupt on new data 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);
readRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val); uint8_t first_spad_to_enable = spad_type_is_aperture ? 12 : 0;
writeRegister(GPIO_HV_MUX_ACTIVE_HIGH_REG, val & ~0x10); // active low uint8_t spads_enabled = 0;
writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); 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++;
}
}
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x01); writeRegisterMulti(GLOBAL_CONFIG_SPAD_ENABLES_REF_0_REG, &ref_spad_map[0], 6);
singleRefCalibration(0x40);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0x02); sensorTuning();
singleRefCalibration(0x00);
writeRegister(SYSTEM_SEQUENCE_CONFIG_REG, 0xE8); // restore config 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
return true; 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
return true;
} }
@@ -834,122 +855,108 @@ bool
VL53LXX::sensorTuning() VL53LXX::sensorTuning()
{ {
writeRegister(0xFF, 0x01); writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x00); 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);
writeRegister(0xFF, 0x00); return true;
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 bool
VL53LXX::singleRefCalibration(uint8_t byte) VL53LXX::singleRefCalibration(uint8_t byte)
{ {
uint8_t val; uint8_t val;
writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP writeRegister(SYSRANGE_START_REG, byte | 0x01); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
do do
{ {
readRegister(RESULT_INTERRUPT_STATUS_REG, val); readRegister(RESULT_INTERRUPT_STATUS_REG, val);
} }
while ((val & 0x07) == 0); while ((val & 0x07) == 0);
writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01); writeRegister(SYSTEM_INTERRUPT_CLEAR_REG, 0x01);
writeRegister(SYSRANGE_START_REG, 0x00);
writeRegister(SYSRANGE_START_REG, 0x00); return true;
return true;
} }
@@ -1039,25 +1046,7 @@ void stop()
void void
test() test()
{ {
struct distance_sensor_s report; //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;
}
int fd = open(VL53LXX_DEVICE_PATH, O_RDONLY); 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); 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 */ /* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
@@ -1116,7 +1102,8 @@ info()
exit(0); exit(0);
} }
} // namespace } // namespace vl53lxx
int int
vl53lxx_main(int argc, char *argv[]) vl53lxx_main(int argc, char *argv[])
@@ -1126,6 +1113,10 @@ vl53lxx_main(int argc, char *argv[])
const char *myoptarg = nullptr; const char *myoptarg = nullptr;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
if (argc < 2) {
goto out;
}
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) { switch (ch) {
case 'R': case 'R':
@@ -1173,6 +1164,8 @@ vl53lxx_main(int argc, char *argv[])
vl53lxx::info(); vl53lxx::info();
} }
out:
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
return PX4_ERROR; return PX4_ERROR;
} }