forked from Archive/PX4-Autopilot
ll40ls: support either internal or external I2C bus
This commit is contained in:
Andrew Tridgell
parent
99bfbb6dc3
commit
c6ada17f68
|
|
@ -74,7 +74,8 @@
|
|||
/* Configuration Constants */
|
||||
#define LL40LS_BUS PX4_I2C_BUS_EXPANSION
|
||||
#define LL40LS_BASEADDR 0x42 /* 7-bit address */
|
||||
#define LL40LS_DEVICE_PATH "/dev/ll40ls"
|
||||
#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int"
|
||||
#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext"
|
||||
|
||||
/* LL40LS Registers addresses */
|
||||
|
||||
|
|
@ -101,7 +102,7 @@ static const int ERROR = -1;
|
|||
class LL40LS : public device::I2C
|
||||
{
|
||||
public:
|
||||
LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
|
||||
LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR);
|
||||
virtual ~LL40LS();
|
||||
|
||||
virtual int init();
|
||||
|
|
@ -134,6 +135,9 @@ private:
|
|||
perf_counter_t _buffer_overflows;
|
||||
uint16_t _last_distance;
|
||||
|
||||
/**< the bus the device is connected to */
|
||||
int _bus;
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
* specific address.
|
||||
|
|
@ -189,8 +193,8 @@ private:
|
|||
*/
|
||||
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
|
||||
|
||||
LL40LS::LL40LS(int bus, int address) :
|
||||
I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
|
||||
LL40LS::LL40LS(int bus, const char *path, int address) :
|
||||
I2C("LL40LS", path, bus, address, 100000),
|
||||
_min_distance(LL40LS_MIN_DISTANCE),
|
||||
_max_distance(LL40LS_MAX_DISTANCE),
|
||||
_reports(nullptr),
|
||||
|
|
@ -202,7 +206,8 @@ LL40LS::LL40LS(int bus, int address) :
|
|||
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")),
|
||||
_last_distance(0)
|
||||
_last_distance(0),
|
||||
_bus(bus)
|
||||
{
|
||||
// up the retries since the device misses the first measure attempts
|
||||
I2C::_retries = 3;
|
||||
|
|
@ -668,55 +673,89 @@ namespace ll40ls
|
|||
#endif
|
||||
const int ERROR = -1;
|
||||
|
||||
LL40LS *g_dev;
|
||||
LL40LS *g_dev_int;
|
||||
LL40LS *g_dev_ext;
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
void info();
|
||||
void start(int bus);
|
||||
void stop(int bus);
|
||||
void test(int bus);
|
||||
void reset(int bus);
|
||||
void info(int bus);
|
||||
void usage();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start()
|
||||
start(int bus)
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
errx(1, "already started");
|
||||
/* create the driver, attempt expansion bus first */
|
||||
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
|
||||
if (g_dev_ext != nullptr)
|
||||
errx(0, "already started external");
|
||||
g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
|
||||
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
|
||||
delete g_dev_ext;
|
||||
g_dev_ext = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new LL40LS(LL40LS_BUS);
|
||||
#ifdef PX4_I2C_BUS_ONBOARD
|
||||
/* if this failed, attempt onboard sensor */
|
||||
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
|
||||
if (g_dev_int != nullptr)
|
||||
errx(0, "already started internal");
|
||||
g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
|
||||
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
|
||||
/* tear down the failing onboard instance */
|
||||
delete g_dev_int;
|
||||
g_dev_int = nullptr;
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
if (bus == PX4_I2C_BUS_ONBOARD) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
|
||||
if (g_dev_int != nullptr) {
|
||||
int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
|
||||
if (fd == -1) {
|
||||
goto fail;
|
||||
}
|
||||
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
|
||||
close(fd);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
|
||||
if (fd < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
goto fail;
|
||||
}
|
||||
if (g_dev_ext != nullptr) {
|
||||
int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
|
||||
if (fd == -1) {
|
||||
goto fail;
|
||||
}
|
||||
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
|
||||
close(fd);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
|
||||
delete g_dev_int;
|
||||
g_dev_int = nullptr;
|
||||
}
|
||||
if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
|
||||
delete g_dev_ext;
|
||||
g_dev_ext = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
|
|
@ -725,11 +764,12 @@ fail:
|
|||
/**
|
||||
* Stop the driver
|
||||
*/
|
||||
void stop()
|
||||
void stop(int bus)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext);
|
||||
if (*g_dev != nullptr) {
|
||||
delete *g_dev;
|
||||
*g_dev = nullptr;
|
||||
|
||||
} else {
|
||||
errx(1, "driver not running");
|
||||
|
|
@ -744,16 +784,17 @@ void stop()
|
|||
* and automatic modes.
|
||||
*/
|
||||
void
|
||||
test()
|
||||
test(int bus)
|
||||
{
|
||||
struct range_finder_report report;
|
||||
ssize_t sz;
|
||||
int ret;
|
||||
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
|
||||
|
||||
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
|
||||
int fd = open(path, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
|
||||
err(1, "%s open failed (try 'll40ls start' if the driver is not running", path);
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
|
|
@ -809,9 +850,10 @@ test()
|
|||
* Reset the driver.
|
||||
*/
|
||||
void
|
||||
reset()
|
||||
reset(int bus)
|
||||
{
|
||||
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
|
||||
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
|
||||
int fd = open(path, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
err(1, "failed ");
|
||||
|
|
@ -832,8 +874,9 @@ reset()
|
|||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
info()
|
||||
info(int bus)
|
||||
{
|
||||
LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
|
@ -844,44 +887,76 @@ info()
|
|||
exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
usage()
|
||||
{
|
||||
warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'");
|
||||
warnx("options:");
|
||||
warnx(" -X only external bus");
|
||||
#ifdef PX4_I2C_BUS_ONBOARD
|
||||
warnx(" -I only internal bus");
|
||||
#endif
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int
|
||||
ll40ls_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
int bus = -1;
|
||||
|
||||
while ((ch = getopt(argc, argv, "XI")) != EOF) {
|
||||
switch (ch) {
|
||||
#ifdef PX4_I2C_BUS_ONBOARD
|
||||
case 'I':
|
||||
bus = PX4_I2C_BUS_ONBOARD;
|
||||
break;
|
||||
#endif
|
||||
case 'X':
|
||||
bus = PX4_I2C_BUS_EXPANSION;
|
||||
break;
|
||||
default:
|
||||
ll40ls::usage();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
const char *verb = argv[optind];
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
ll40ls::start();
|
||||
if (!strcmp(verb, "start")) {
|
||||
ll40ls::start(bus);
|
||||
}
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
ll40ls::stop();
|
||||
if (!strcmp(verb, "stop")) {
|
||||
ll40ls::stop(bus);
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
ll40ls::test();
|
||||
if (!strcmp(verb, "test")) {
|
||||
ll40ls::test(bus);
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset")) {
|
||||
ll40ls::reset();
|
||||
if (!strcmp(verb, "reset")) {
|
||||
ll40ls::reset(bus);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
|
||||
ll40ls::info();
|
||||
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
|
||||
ll40ls::info(bus);
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
|
|
|
|||
Loading…
Reference in New Issue