dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ll40ls: support either internal or external I2C bus

This commit is contained in:
Andrew Tridgell 2014-10-20 07:05:58 +11:00
parent 99bfbb6dc3
commit c6ada17f68
1 changed files with 131 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

187
src/drivers/ll40ls/ll40ls.cpp
View File

@ -74,7 +74,8 @@
/* Configuration Constants */ /* Configuration Constants */
#define LL40LS_BUS PX4_I2C_BUS_EXPANSION #define LL40LS_BUS PX4_I2C_BUS_EXPANSION
#define LL40LS_BASEADDR 0x42 /* 7-bit address */ #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 */ /* LL40LS Registers addresses */
@ -101,7 +102,7 @@ static const int ERROR = -1;
class LL40LS : public device::I2C class LL40LS : public device::I2C
{ {
public: public:
LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR); LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR);
virtual ~LL40LS(); virtual ~LL40LS();
virtual int init(); virtual int init();
@ -134,6 +135,9 @@ private:
perf_counter_t _buffer_overflows; perf_counter_t _buffer_overflows;
uint16_t _last_distance; 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 * Test whether the device supported by the driver is present at a
* specific address. * specific address.
@ -189,8 +193,8 @@ private:
*/ */
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
LL40LS::LL40LS(int bus, int address) : LL40LS::LL40LS(int bus, const char *path, int address) :
I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000), I2C("LL40LS", path, bus, address, 100000),
_min_distance(LL40LS_MIN_DISTANCE), _min_distance(LL40LS_MIN_DISTANCE),
_max_distance(LL40LS_MAX_DISTANCE), _max_distance(LL40LS_MAX_DISTANCE),
_reports(nullptr), _reports(nullptr),
@ -202,7 +206,8 @@ LL40LS::LL40LS(int bus, int address) :
_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")),
_last_distance(0) _last_distance(0),
_bus(bus)
{ {
// up the retries since the device misses the first measure attempts // up the retries since the device misses the first measure attempts
I2C::_retries = 3; I2C::_retries = 3;
@ -668,55 +673,89 @@ namespace ll40ls
#endif #endif
const int ERROR = -1; const int ERROR = -1;
LL40LS *g_dev; LL40LS *g_dev_int;
LL40LS *g_dev_ext;
void start(); void start(int bus);
void stop(); void stop(int bus);
void test(); void test(int bus);
void reset(); void reset(int bus);
void info(); void info(int bus);
void usage();
/** /**
* Start the driver. * Start the driver.
*/ */
void void
start() start(int bus)
{ {
int fd; /* create the driver, attempt expansion bus first */
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev != nullptr) { if (g_dev_ext != nullptr)
errx(1, "already started"); 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 */ #ifdef PX4_I2C_BUS_ONBOARD
g_dev = new LL40LS(LL40LS_BUS); /* 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) { if (bus == PX4_I2C_BUS_ONBOARD) {
goto fail; goto fail;
} }
}
if (OK != g_dev->init()) { if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail; goto fail;
}
} }
#endif
/* set the poll rate to default, starts automatic data collection */ /* 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) { if (g_dev_ext != nullptr) {
goto fail; int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
} if (fd == -1) {
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { }
goto fail; int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
} close(fd);
if (ret < 0) {
goto fail;
}
}
exit(0); exit(0);
fail: fail:
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
if (g_dev != nullptr) { delete g_dev_int;
delete g_dev; g_dev_int = nullptr;
g_dev = 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"); errx(1, "driver start failed");
@ -725,11 +764,12 @@ fail:
/** /**
* Stop the driver * Stop the driver
*/ */
void stop() void stop(int bus)
{ {
if (g_dev != nullptr) { LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext);
delete g_dev; if (*g_dev != nullptr) {
g_dev = nullptr; delete *g_dev;
*g_dev = nullptr;
} else { } else {
errx(1, "driver not running"); errx(1, "driver not running");
@ -744,16 +784,17 @@ void stop()
* and automatic modes. * and automatic modes.
*/ */
void void
test() 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);
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); int fd = open(path, O_RDONLY);
if (fd < 0) { 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 */ /* do a simple demand read */
@ -809,9 +850,10 @@ test()
* Reset the driver. * Reset the driver.
*/ */
void 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) { if (fd < 0) {
err(1, "failed "); err(1, "failed ");
@ -832,8 +874,9 @@ reset()
* Print a little info about the driver. * Print a little info about the driver.
*/ */
void void
info() info(int bus)
{ {
LL40LS *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");
} }
@ -844,44 +887,76 @@ info()
exit(0); 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 } // namespace
int int
ll40ls_main(int argc, char *argv[]) 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. * Start/load the driver.
*/ */
if (!strcmp(argv[1], "start")) { if (!strcmp(verb, "start")) {
ll40ls::start(); ll40ls::start(bus);
} }
/* /*
* Stop the driver * Stop the driver
*/ */
if (!strcmp(argv[1], "stop")) { if (!strcmp(verb, "stop")) {
ll40ls::stop(); ll40ls::stop(bus);
} }
/* /*
* Test the driver/device. * Test the driver/device.
*/ */
if (!strcmp(argv[1], "test")) { if (!strcmp(verb, "test")) {
ll40ls::test(); ll40ls::test(bus);
} }
/* /*
* Reset the driver. * Reset the driver.
*/ */
if (!strcmp(argv[1], "reset")) { if (!strcmp(verb, "reset")) {
ll40ls::reset(); ll40ls::reset(bus);
} }
/* /*
* Print driver information. * Print driver information.
*/ */
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ll40ls::info(); ll40ls::info(bus);
} }
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");