From 21f97cfce68e4f89b9f6d2dfd4dc3aa8e9363062 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 19 Sep 2017 13:26:20 +0200 Subject: [PATCH] LidarLite: update for new orientation convention add possibility to specify orientation and adapt to new defaults and use px4_getopt --- src/drivers/ll40ls/LidarLiteI2C.cpp | 5 +++-- src/drivers/ll40ls/LidarLiteI2C.h | 4 +++- src/drivers/ll40ls/LidarLitePWM.cpp | 5 +++-- src/drivers/ll40ls/LidarLitePWM.h | 3 ++- src/drivers/ll40ls/ll40ls.cpp | 30 +++++++++++++++++++---------- 5 files changed, 31 insertions(+), 16 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 6078f12741..84f73fba68 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -49,8 +49,9 @@ #include #include -LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : +LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address, uint8_t rotation) : I2C("LL40LS", path, bus, address, 100000), + _rotation(rotation), _work{}, _reports(nullptr), _sensor_ok(false), @@ -454,7 +455,7 @@ int LidarLiteI2C::collect() report.covariance = 0.0f; /* the sensor is in fact a laser + sonar but there is no enum for this */ report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.orientation = 8; + report.orientation = _rotation; /* TODO: set proper ID */ report.id = 0; diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index b031cf2530..d6592b4a37 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -71,7 +71,8 @@ class LidarLiteI2C : public LidarLite, public device::I2C { public: - LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); + LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR, + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); virtual ~LidarLiteI2C(); int init() override; @@ -100,6 +101,7 @@ protected: int reset_sensor(); private: + uint8_t _rotation; work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 5713322b28..0619c00f60 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -49,8 +49,9 @@ #include #include -LidarLitePWM::LidarLitePWM(const char *path) : +LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) : CDev("LidarLitePWM", path), + _rotation(rotation), _work{}, _reports(nullptr), _class_instance(-1), @@ -178,7 +179,7 @@ int LidarLitePWM::measure() _range.min_distance = get_minimum_distance(); _range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ _range.covariance = 0.0f; - _range.orientation = 8; + _range.orientation = _rotation; /* TODO: set proper ID */ _range.id = 0; diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 503861d8a1..df4e338611 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -60,7 +60,7 @@ class LidarLitePWM : public LidarLite, public device::CDev { public: - LidarLitePWM(const char *path); + LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); virtual ~LidarLitePWM(); int init() override; @@ -107,6 +107,7 @@ protected: void task_main_trampoline(int argc, char *argv[]); private: + uint8_t _rotation; work_s _work; ringbuffer::RingBuffer *_reports; int _class_instance; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index b33e189462..57be0e99b5 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -93,7 +94,7 @@ namespace ll40ls LidarLite *instance = nullptr; -void start(enum LL40LS_BUS busid); +void start(enum LL40LS_BUS busid, uint8_t rotation); void stop(); void test(); void reset(); @@ -104,7 +105,7 @@ void usage(); /** * Start the driver. */ -void start(enum LL40LS_BUS busid) +void start(enum LL40LS_BUS busid, uint8_t rotation) { int fd, ret; @@ -113,7 +114,7 @@ void start(enum LL40LS_BUS busid) } if (busid == LL40LS_BUS_PWM) { - instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM); + instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation); if (!instance) { warnx("Failed to instantiate LidarLitePWM"); @@ -131,7 +132,7 @@ void start(enum LL40LS_BUS busid) continue; } - instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname); + instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation); if (!instance) { warnx("Failed to instantiate LidarLiteI2C"); @@ -340,6 +341,7 @@ usage() #ifdef PX4_I2C_BUS_ONBOARD warnx(" -I only internal bus"); #endif + warnx("E.g. ll40ls start i2c -R 0"); } } // namespace @@ -348,9 +350,12 @@ int ll40ls_main(int argc, char *argv[]) { int ch; + int myoptind = 1; + const char *myoptarg = NULL; enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL; + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - while ((ch = getopt(argc, argv, "XI")) != EOF) { + while ((ch = px4_getopt(argc, argv, "IXR:", &myoptind, &myoptarg)) != EOF) { switch (ch) { #ifdef PX4_I2C_BUS_ONBOARD @@ -363,6 +368,11 @@ ll40ls_main(int argc, char *argv[]) busid = LL40LS_BUS_I2C_EXTERNAL; break; + case 'R': + rotation = (uint8_t)atoi(myoptarg); + PX4_INFO("Setting Lidar orientation to %d", (int)rotation); + break; + default: ll40ls::usage(); return 0; @@ -370,8 +380,8 @@ ll40ls_main(int argc, char *argv[]) } /* determine protocol first because it's needed next */ - if (argc > optind + 1) { - const char *protocol = argv[optind + 1]; + if (argc > myoptind + 1) { + const char *protocol = argv[myoptind + 1]; if (!strcmp(protocol, "pwm")) { busid = LL40LS_BUS_PWM;; @@ -387,11 +397,11 @@ ll40ls_main(int argc, char *argv[]) } /* now determine action */ - if (argc > optind) { - const char *verb = argv[optind]; + if (argc > myoptind) { + const char *verb = argv[myoptind]; if (!strcmp(verb, "start")) { - ll40ls::start(busid); + ll40ls::start(busid, rotation); } else if (!strcmp(verb, "stop")) { ll40ls::stop();