LidarLite port to raspberrypi (#8633)

src/drivers/ll40ls/LidarLite.cpp

@@ -38,9 +38,8 @@
  *
  * Generic interface driver for the PulsedLight Lidar-Lite range finders.
  */
+
 #include "LidarLite.h"
-#include <errno.h>
-#include <nuttx/clock.h>
 
 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) {
 

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;
 

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<struct distance_sensor_s *>(buffer);

src/drivers/ll40ls/LidarLiteI2C.h

@@ -42,8 +42,8 @@
 
 #include "LidarLite.h"
 
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
+#include <px4_workqueue.h>
+
 #include <systemlib/perf_counter.h>
 
 #include <drivers/device/i2c.h>
@@ -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

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<struct distance_sensor_s *>(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 */

src/drivers/ll40ls/LidarLitePWM.h

@@ -45,8 +45,8 @@
 
 #include "LidarLite.h"
 
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
+#include <px4_workqueue.h>
+
 
 #include <drivers/device/ringbuffer.h>
 #include <systemlib/perf_counter.h>
@@ -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;
 

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;