From 1037e983abb95e8689018c00df31c690e39c47ac Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 12 Jan 2018 16:20:00 -0500 Subject: [PATCH] LidarLite port to raspberrypi (#8633) --- cmake/configs/posix_rpi_common.cmake | 3 +- cmake/configs/posix_sitl_default.cmake | 11 +-- src/drivers/ll40ls/LidarLite.cpp | 5 +- src/drivers/ll40ls/LidarLite.h | 2 +- src/drivers/ll40ls/LidarLiteI2C.cpp | 23 +++-- src/drivers/ll40ls/LidarLiteI2C.h | 18 ++-- src/drivers/ll40ls/LidarLitePWM.cpp | 5 +- src/drivers/ll40ls/LidarLitePWM.h | 8 +- src/drivers/ll40ls/ll40ls.cpp | 114 ++++++++++++------------- 9 files changed, 90 insertions(+), 99 deletions(-) diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index 9658c8ffb6..b9f7b336e6 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -15,9 +15,10 @@ set(config_module_list # # Board support modules # - drivers/device drivers/airspeed + drivers/device drivers/ets_airspeed + drivers/ll40ls drivers/ms4525_airspeed drivers/ms5525_airspeed drivers/sdp3x_airspeed diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 7448033ddf..6807f56a43 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -2,18 +2,19 @@ set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) set(config_module_list + drivers/airspeed drivers/boards drivers/camera_trigger drivers/device - drivers/gps - drivers/pwm_out_sim - drivers/vmount - drivers/linux_gpio - drivers/airspeed drivers/ets_airspeed + drivers/gps + drivers/linux_gpio + drivers/ll40ls drivers/ms4525_airspeed drivers/ms5525_airspeed + drivers/pwm_out_sim drivers/sdp3x_airspeed + drivers/vmount modules/sensors platforms/posix/drivers/accelsim diff --git a/src/drivers/ll40ls/LidarLite.cpp b/src/drivers/ll40ls/LidarLite.cpp index e3bb7dccfc..657376091d 100644 --- a/src/drivers/ll40ls/LidarLite.cpp +++ b/src/drivers/ll40ls/LidarLite.cpp @@ -38,9 +38,8 @@ * * Generic interface driver for the PulsedLight Lidar-Lite range finders. */ + #include "LidarLite.h" -#include -#include LidarLite::LidarLite() : _min_distance(LL40LS_MIN_DISTANCE), @@ -78,7 +77,7 @@ uint32_t LidarLite::getMeasureTicks() const return _measure_ticks; } -int LidarLite::ioctl(struct file *filp, int cmd, unsigned long arg) +int LidarLite::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h index 2300e0cba7..2c00a92bf0 100644 --- a/src/drivers/ll40ls/LidarLite.h +++ b/src/drivers/ll40ls/LidarLite.h @@ -62,7 +62,7 @@ public: virtual int init() = 0; - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual void start() = 0; diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index b7f5db53aa..5c829e85ab 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -73,9 +73,6 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr // up the retries since the device misses the first measure attempts _retries = 3; - // enable debug() calls - _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -145,7 +142,7 @@ int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val) { const uint8_t cmd[2] = { reg, val }; - return transfer(&cmd[0], 2, NULL, 0); + return transfer(&cmd[0], 2, nullptr, 0); } /* @@ -154,16 +151,16 @@ int LidarLiteI2C::write_reg(uint8_t reg, uint8_t val) */ int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - if (send != NULL && send_len > 0) { - int ret = transfer(send, send_len, NULL, 0); + if (send != nullptr && send_len > 0) { + int ret = transfer(send, send_len, nullptr, 0); if (ret != OK) { return ret; } } - if (recv != NULL && recv_len > 0) { - return transfer(NULL, 0, recv, recv_len); + if (recv != nullptr && recv_len > 0) { + return transfer(nullptr, 0, recv, recv_len); } return OK; @@ -200,7 +197,7 @@ ok: return reset_sensor(); } -int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) +int LidarLiteI2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSQUEUEDEPTH: { @@ -209,14 +206,14 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } - irqstate_t flags = px4_enter_critical_section(); + ATOMIC_ENTER; if (!_reports->resize(arg)) { - px4_leave_critical_section(flags); + ATOMIC_LEAVE; return -ENOMEM; } - px4_leave_critical_section(flags); + ATOMIC_LEAVE; return OK; } @@ -233,7 +230,7 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) } } -ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) +ssize_t LidarLiteI2C::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct distance_sensor_s); struct distance_sensor_s *rbuf = reinterpret_cast(buffer); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index b1446e7c36..e99042897f 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -42,8 +42,8 @@ #include "LidarLite.h" -#include -#include +#include + #include #include @@ -78,8 +78,8 @@ public: int init() override; - ssize_t read(struct file *filp, char *buffer, size_t buflen); - int ioctl(struct file *filp, int cmd, unsigned long arg) override; + ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; /** * Diagnostics - print some basic information about the driver. @@ -94,12 +94,12 @@ public: const char *get_dev_name() override; protected: - int probe(); + int probe() override; int read_reg(uint8_t reg, uint8_t &val); int write_reg(uint8_t reg, uint8_t val); int measure() override; - int reset_sensor(); + int reset_sensor() override; private: uint8_t _rotation; @@ -151,19 +151,19 @@ private: * @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. */ - void start(); + void start() override; /** * Stop the automatic measurement state machine. */ - void stop(); + void stop() override; /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ void cycle(); - int collect(); + int collect() override; /** * Static trampoline from the workq context; because we don't have a diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 0619c00f60..12bd237ea9 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -159,7 +159,6 @@ void LidarLitePWM::cycle() (worker_t)&LidarLitePWM::cycle_trampoline, this, getMeasureTicks()); - return; } int LidarLitePWM::measure() @@ -201,7 +200,7 @@ int LidarLitePWM::measure() return PX4_OK; } -ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen) +ssize_t LidarLitePWM::read(device::file_t *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct distance_sensor_s); struct distance_sensor_s *rbuf = reinterpret_cast(buffer); @@ -243,7 +242,7 @@ ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen) } int -LidarLitePWM::ioctl(struct file *filp, int cmd, unsigned long arg) +LidarLitePWM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { /* no custom ioctls implemented for now */ diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index df4e338611..2d6954b15c 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -45,8 +45,8 @@ #include "LidarLite.h" -#include -#include +#include + #include #include @@ -65,8 +65,8 @@ public: int init() override; - ssize_t read(struct file *filp, char *buffer, size_t buflen) override; - int ioctl(struct file *filp, int cmd, unsigned long arg); + ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; void start() override; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 57be0e99b5..579d7198a5 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -110,19 +110,19 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) int fd, ret; if (instance) { - warnx("driver already started"); + PX4_INFO("driver already started"); } if (busid == LL40LS_BUS_PWM) { instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation); if (!instance) { - warnx("Failed to instantiate LidarLitePWM"); + PX4_ERR("Failed to instantiate LidarLitePWM"); return; } if (instance->init() != PX4_OK) { - warnx("failed to initialize LidarLitePWM"); + PX4_ERR("failed to initialize LidarLitePWM"); goto fail; } @@ -135,7 +135,7 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation); if (!instance) { - warnx("Failed to instantiate LidarLiteI2C"); + PX4_ERR("Failed to instantiate LidarLiteI2C"); return; } @@ -143,29 +143,29 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) break; } - warnx("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum); + PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum); delete instance; instance = nullptr; } } if (!instance) { - warnx("No LidarLite found"); + PX4_WARN("No LidarLite found"); return; } - fd = open(instance->get_dev_name(), O_RDONLY); + fd = px4_open(instance->get_dev_name(), O_RDONLY); if (fd == -1) { - warnx("Error opening fd"); + PX4_ERR("Error opening fd"); goto fail; } - ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); + ret = px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + px4_close(fd); if (ret < 0) { - warnx("pollrate fail"); + PX4_ERR("pollrate fail"); goto fail; } @@ -198,76 +198,70 @@ test() int ret; if (!instance) { - warnx("No ll40ls driver running"); - errx(1, "FAIL"); + PX4_ERR("No ll40ls driver running"); + return; } - int fd = open(instance->get_dev_name(), O_RDONLY); + int fd = px4_open(instance->get_dev_name(), O_RDONLY); if (fd < 0) { - warnx("Error opening fd"); - errx(1, "FAIL"); + PX4_ERR("Error opening fd"); + return; } /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); + sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warnx("immediate read failed"); - goto error; + PX4_ERR("immediate read failed"); + return; } - warnx("single read"); - warnx("measurement: %0.2f m", (double)report.current_distance); - warnx("time: %lld", report.timestamp); + PX4_INFO("single read"); + PX4_INFO("measurement: %0.2f m", (double)report.current_distance); + PX4_INFO("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ - if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - warnx("failed to set 2Hz poll rate"); - goto error; + if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_ERR("failed to set 2Hz poll rate"); + return; } /* read the sensor 5 times and report each value */ for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; + px4_pollfd_struct_t fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + ret = px4_poll(&fds, 1, 2000); if (ret != 1) { - warnx("timed out waiting for sensor data"); - goto error; + PX4_WARN("timed out waiting for sensor data"); + return; } /* now go get it */ - sz = read(fd, &report, sizeof(report)); + sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warnx("periodic read failed"); - goto error; + PX4_WARN("periodic read failed"); + return; } - warnx("periodic read %u", i); - warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); - warnx("measurement: %0.3f m", (double)report.current_distance); - warnx("time: %lld", report.timestamp); + PX4_INFO("periodic read %u", i); + PX4_INFO("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + PX4_INFO("measurement: %0.3f m", (double)report.current_distance); + PX4_INFO("time: %lld", report.timestamp); } /* reset the sensor polling to default rate */ - if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - warnx("failed to set default poll rate"); - goto error; + if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_WARN("failed to set default poll rate"); } - close(fd); - errx(0, "PASS"); - -error: - close(fd); - errx(1, "FAIL"); + px4_close(fd); } /** @@ -277,29 +271,29 @@ void reset() { if (!instance) { - warnx("No ll40ls driver running"); + PX4_WARN("No ll40ls driver running"); return; } - int fd = open(instance->get_dev_name(), O_RDONLY); + int fd = px4_open(instance->get_dev_name(), O_RDONLY); if (fd < 0) { - warnx("Error opening fd"); + PX4_ERR("Error opening fd"); return; } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - warnx("driver reset failed"); + if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); goto error; } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - warnx("driver poll restart failed"); + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); goto error; } error: - close(fd); + px4_close(fd); } /** @@ -335,13 +329,13 @@ regdump() void usage() { - warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]"); - warnx("options for I2C:"); - warnx(" -X only external bus"); + PX4_INFO("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]"); + PX4_INFO("options for I2C:"); + PX4_INFO(" -X only external bus"); #ifdef PX4_I2C_BUS_ONBOARD - warnx(" -I only internal bus"); + PX4_INFO(" -I only internal bus"); #endif - warnx("E.g. ll40ls start i2c -R 0"); + PX4_INFO("E.g. ll40ls start i2c -R 0"); } } // namespace @@ -351,7 +345,7 @@ ll40ls_main(int argc, char *argv[]) { int ch; int myoptind = 1; - const char *myoptarg = NULL; + const char *myoptarg = nullptr; enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL; uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;