From 0ab0de5805670dbceb7be497502e0b1c5c7677ca Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:42:14 +0100 Subject: [PATCH] ll40ls: adapt the cli interface, so that the commands work with the PWM and I2C driver --- src/drivers/ll40ls/ll40ls.cpp | 293 +++++++++++++++++++++------------- 1 file changed, 184 insertions(+), 109 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 6ef2da94e7..9c52ce6e9a 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -35,11 +35,13 @@ * @file ll40ls.cpp * @author Allyson Kreft * @author Johan Jansen + * @author Ban Siesta * * Interface for the PulsedLight Lidar-Lite range finders. */ #include "LidarLiteI2C.h" +#include "LidarLitePWM.h" #include #include #include @@ -53,6 +55,7 @@ #define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" #define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" +#define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm" /* * Driver 'main' command. @@ -74,93 +77,122 @@ const int ERROR = -1; LidarLiteI2C *g_dev_int; LidarLiteI2C *g_dev_ext; +LidarLitePWM *g_dev_pwm; -void start(int bus); -void stop(int bus); -void test(int bus); -void reset(int bus); -void info(int bus); -void regdump(int bus); +void start(const bool use_i2c, const int bus); +void stop(const bool use_i2c, const int bus); +void test(const bool use_i2c, const int bus); +void reset(const bool use_i2c, const int bus); +void info(const bool use_i2c, const int bus); +void regdump(const bool use_i2c, const int bus); void usage(); /** * Start the driver. */ -void start(int bus) +void start(const bool use_i2c, const int bus) { - /* 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"); - } + if (use_i2c) { + /* 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 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()) { - delete g_dev_ext; - g_dev_ext = nullptr; + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; - if (bus == PX4_I2C_BUS_EXPANSION) { - goto fail; + if (bus == PX4_I2C_BUS_EXPANSION) { + goto fail; + } } } - } #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"); - } + /* 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 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()) { - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; + 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 (bus == PX4_I2C_BUS_ONBOARD) { + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + + if (g_dev_int == nullptr && 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 */ - if (g_dev_int != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); + /* set the poll rate to default, starts automatic data collection */ + if (g_dev_int != nullptr) { + 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); + close(fd); + + if (ret < 0) { + goto fail; + } } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); + if (g_dev_ext != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); - if (ret < 0) { - goto fail; - } - } + if (fd == -1) { + goto fail; + } - if (g_dev_ext != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); - if (fd == -1) { - goto fail; + if (ret < 0) { + goto fail; + } } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); + } else { + g_dev_pwm = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM); - if (ret < 0) { - goto fail; + if (g_dev_pwm != nullptr && OK != g_dev_pwm->init()) { + delete g_dev_pwm; + g_dev_pwm = nullptr; + warnx("failed to init PWM"); + } + + if (g_dev_pwm != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_PWM, O_RDONLY); + + if (fd == -1) { + warnx("fd nothing"); + goto fail; + } + + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + + if (ret < 0) { + warnx("pollrate fail"); + goto fail; + } } } @@ -184,7 +216,7 @@ fail: /** * Stop the driver */ -void stop(int bus) +void stop(const bool use_i2c, const int bus) { LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext); @@ -205,17 +237,25 @@ void stop(int bus) * and automatic modes. */ void -test(int bus) +test(const bool use_i2c, const 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); + + const char *path; + + if (use_i2c) { + path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + } else { + path = LL40LS_DEVICE_PATH_PWM; + } int fd = open(path, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'll40ls start' if the driver is not running", path); + err(1, "%s open failed, is the driver running?", path); } /* do a simple demand read */ @@ -234,7 +274,7 @@ test(int bus) errx(1, "failed to set 2Hz poll rate"); } - /* read the sensor 5x and report each value */ + /* read the sensor 5 times and report each value */ for (unsigned i = 0; i < 5; i++) { struct pollfd fds; @@ -255,7 +295,7 @@ test(int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + warnx("measurement: %0.3f m", (double)report.distance); warnx("time: %lld", report.timestamp); } @@ -271,9 +311,18 @@ test(int bus) * Reset the driver. */ void -reset(int bus) +reset(const bool use_i2c, const int bus) { - const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + const char *path; + + if (use_i2c) { + path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + } else { + path = LL40LS_DEVICE_PATH_PWM; + } + int fd = open(path, O_RDONLY); if (fd < 0) { @@ -295,12 +344,19 @@ reset(int bus) * Print a little info about the driver. */ void -info(int bus) +info(const bool use_i2c, const int bus) { - LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + LidarLite *g_dev = nullptr; - if (g_dev == nullptr) { - errx(1, "driver not running"); + if (use_i2c) { + g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + } else { + g_dev = g_dev_pwm; } printf("state @ %p\n", g_dev); @@ -313,7 +369,7 @@ info(int bus) * Dump registers */ void -regdump(int bus) +regdump(const bool use_i2c, const int bus) { LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); @@ -330,8 +386,8 @@ regdump(int bus) void usage() { - warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump'"); - warnx("options:"); + warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]"); + warnx("options for I2C:"); warnx(" -X only external bus"); #ifdef PX4_I2C_BUS_ONBOARD warnx(" -I only internal bus"); @@ -365,49 +421,68 @@ ll40ls_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + /* default to I2C if no protocol is given */ + bool use_i2c = true; - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - ll40ls::start(bus); + /* determine protocol first because it's needed next */ + if (argc > optind + 1) { + const char *protocol = argv[optind + 1]; + + if (!strcmp(protocol, "pwm")) { + use_i2c = false; + + } else if (!strcmp(protocol, "i2c")) { + use_i2c = true; + + } else { + warnx("unknown protocol, choose pwm or i2c"); + ll40ls::usage(); + exit(0); + } } - /* - * Stop the driver - */ - if (!strcmp(verb, "stop")) { - ll40ls::stop(bus); + /* now determine action */ + if (argc > optind) { + const char *verb = argv[optind]; + + /* Start/load the driver. */ + if (!strcmp(verb, "start")) { + + ll40ls::start(use_i2c, bus); + } + + /* Stop the driver */ + if (!strcmp(verb, "stop")) { + ll40ls::stop(use_i2c, bus); + } + + /* Test the driver/device. */ + else if (!strcmp(verb, "test")) { + ll40ls::test(use_i2c, bus); + } + + /* Reset the driver. */ + else if (!strcmp(verb, "reset")) { + ll40ls::reset(use_i2c, bus); + } + + /* dump registers */ + else if (!strcmp(verb, "regdump")) { + ll40ls::regdump(use_i2c, bus); + } + + /* Print driver information. */ + else if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + ll40ls::info(use_i2c, bus); + } + + else { + ll40ls::usage(); + exit(0); + } } - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - ll40ls::test(bus); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - ll40ls::reset(bus); - } - - /* - * dump registers - */ - if (!strcmp(verb, "regdump")) { - ll40ls::regdump(bus); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - ll40ls::info(bus); - } - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + ll40ls::usage(); + exit(0); }