fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 13:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ll40ls: astyle

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