bmi160 move to PX4Accelerometer and PX4Gyroscope and cleanup (#12128)

src/drivers/imu/bmi160/CMakeLists.txt

@@ -38,6 +38,9 @@ px4_add_module(
 		-Wno-cast-align # TODO: fix and enable
 	SRCS
 		bmi160.cpp
-		bmi160_gyro.cpp
 		bmi160_main.cpp
+	DEPENDS
+		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
 	)

src/drivers/imu/bmi160/bmi160.hpp

@@ -2,51 +2,18 @@
 #define BMI160_HPP_
 
 #include <px4_config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
 #include <perf/perf_counter.h>
-#include <systemlib/err.h>
 #include <systemlib/conversions.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/clock.h>
-
-#include <board_config.h>
 #include <drivers/drv_hrt.h>
-
 #include <drivers/device/spi.h>
-#include <drivers/device/ringbuffer.h>
-#include <drivers/device/integrator.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/conversion/rotation.h>
 #include <px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 
 #define DIR_READ                0x80
 #define DIR_WRITE               0x00
 
-#define BMI160_DEVICE_PATH_ACCEL    "/dev/bmi160_accel"
-#define BMI160_DEVICE_PATH_GYRO		"/dev/bmi160_gyro"
-#define BMI160_DEVICE_PATH_MAG		"/dev/bmi160_mag"
-#define BMI160_DEVICE_PATH_ACCEL_EXT    "/dev/bmi160_accel_ext"
-#define BMI160_DEVICE_PATH_GYRO_EXT		"/dev/bmi160_gyro_ext"
-#define BMI160_DEVICE_PATH_MAG_EXT	"/dev/bmi160_mag_ext"
-
 // BMI 160 registers
 
 #define BMIREG_CHIP_ID          0x00
@@ -242,19 +209,16 @@
 
 #define BMI160_TIMER_REDUCTION				200
 
-class BMI160_gyro;
+using namespace time_literals;
 
 class BMI160 : public device::SPI, public px4::ScheduledWorkItem
 {
 public:
-	BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation);
+	BMI160(int bus, uint32_t device, enum Rotation rotation);
 	virtual ~BMI160();
 
 	virtual int		init();
 
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
 	/**
 	 * Diagnostics - print some basic information about the driver.
 	 */
@@ -268,36 +232,18 @@ public:
 protected:
 	virtual int		probe();
 
-	friend class BMI160_gyro;
-
-	virtual ssize_t		gyro_read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
-
 private:
-	BMI160_gyro		*_gyro;
+
+	PX4Accelerometer	_px4_accel;
+	PX4Gyroscope		_px4_gyro;
+
 	uint8_t			_whoami;	/** whoami result */
 
-	unsigned		_call_interval;
-
-	ringbuffer::RingBuffer	*_accel_reports;
-
-	struct accel_calibration_s	_accel_scale;
-	float			_accel_range_scale;
-	float			_accel_range_m_s2;
-	orb_advert_t		_accel_topic;
-	int			_accel_orb_class_instance;
-	int			_accel_class_instance;
-
-	ringbuffer::RingBuffer	*_gyro_reports;
-
-	struct gyro_calibration_s	_gyro_scale;
-	float			_gyro_range_scale;
-	float			_gyro_range_rad_s;
-
 	unsigned		_dlpf_freq;
 
-	float		_accel_sample_rate;
+	float			_accel_sample_rate{BMI160_ACCEL_DEFAULT_RATE};
-	float		_gyro_sample_rate;
+	float			_gyro_sample_rate{BMI160_GYRO_DEFAULT_RATE};
+
 	perf_counter_t		_accel_reads;
 	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
@@ -307,36 +253,21 @@ private:
 	perf_counter_t		_reset_retries;
 	perf_counter_t		_duplicates;
 
-	uint8_t			_register_wait;
+	uint8_t			_register_wait{0};
-	uint64_t		_reset_wait;
+	uint64_t		_reset_wait{0};
-
-	math::LowPassFilter2p	_accel_filter_x;
-	math::LowPassFilter2p	_accel_filter_y;
-	math::LowPassFilter2p	_accel_filter_z;
-	math::LowPassFilter2p	_gyro_filter_x;
-	math::LowPassFilter2p	_gyro_filter_y;
-	math::LowPassFilter2p	_gyro_filter_z;
-
-	Integrator		_accel_int;
-	Integrator		_gyro_int;
-
-	enum Rotation		_rotation;
 
 	// this is used to support runtime checking of key
 	// configuration registers to detect SPI bus errors and sensor
 	// reset
-#define BMI160_NUM_CHECKED_REGISTERS 10
+	static constexpr int BMI160_NUM_CHECKED_REGISTERS{10};
 	static const uint8_t	_checked_registers[BMI160_NUM_CHECKED_REGISTERS];
 	uint8_t			_checked_values[BMI160_NUM_CHECKED_REGISTERS];
 	uint8_t			_checked_bad[BMI160_NUM_CHECKED_REGISTERS];
-	uint8_t			_checked_next;
+	uint8_t			_checked_next{0};
-
-	// last temperature reading for print_info()
-	float			_last_temperature;
 
 	// keep last accel reading for duplicate detection
-	uint16_t		_last_accel[3];
+	uint16_t		_last_accel[3] {};
-	bool			_got_duplicate;
+	bool			_got_duplicate{false};
 
 	/**
 	 * Start automatic measurement.
@@ -413,13 +344,6 @@ private:
 	 */
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
-	/**
-	 * Measurement self test
-	 *
-	 * @return 0 on success, 1 on failure
-	 */
-	int 			self_test();
-
 	/*
 	  set low pass filter frequency
 	 */

src/drivers/imu/bmi160/bmi160_gyro.cpp

@@ -1,63 +0,0 @@
-
-#include "bmi160_gyro.hpp"
-#include "bmi160.hpp"
-
-BMI160_gyro::BMI160_gyro(BMI160 *parent, const char *path) : CDev("BMI160_gyro", path),
-	_parent(parent),
-	_gyro_topic(nullptr),
-	_gyro_orb_class_instance(-1),
-	_gyro_class_instance(-1)
-{
-}
-
-BMI160_gyro::~BMI160_gyro()
-{
-	if (_gyro_class_instance != -1) {
-		unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
-	}
-}
-
-int
-BMI160_gyro::init()
-{
-	int ret;
-
-	// do base class init
-	ret = CDev::init();
-
-	/* if probe/setup failed, bail now */
-	if (ret != OK) {
-		DEVICE_DEBUG("gyro init failed");
-		return ret;
-	}
-
-	_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
-
-	return ret;
-}
-
-void
-BMI160_gyro::parent_poll_notify()
-{
-	poll_notify(POLLIN);
-}
-
-ssize_t
-BMI160_gyro::read(struct file *filp, char *buffer, size_t buflen)
-{
-	return _parent->gyro_read(filp, buffer, buflen);
-}
-
-int
-BMI160_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-
-	switch (cmd) {
-	case DEVIOCGDEVICEID:
-		return (int)CDev::ioctl(filp, cmd, arg);
-		break;
-
-	default:
-		return _parent->gyro_ioctl(filp, cmd, arg);
-	}
-}

src/drivers/imu/bmi160/bmi160_gyro.hpp

@@ -1,41 +0,0 @@
-#ifndef BMI160_GYRO_HPP_
-#define BMI160_GYRO_HPP_
-
-#include <px4_config.h>
-
-#include "bmi160.hpp"
-
-/**
- * Helper class implementing the gyro driver node.
- */
-class BMI160_gyro : public device::CDev
-{
-public:
-	BMI160_gyro(BMI160 *parent, const char *path);
-	~BMI160_gyro();
-
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	virtual int		init();
-
-protected:
-	friend class BMI160;
-
-	void			parent_poll_notify();
-
-private:
-	BMI160			*_parent;
-	orb_advert_t		_gyro_topic;
-	int			_gyro_orb_class_instance;
-	int			_gyro_class_instance;
-
-	/* do not allow to copy this class due to pointer data members */
-	BMI160_gyro(const BMI160_gyro &);
-	BMI160_gyro operator=(const BMI160_gyro &);
-};
-
-
-
-
-#endif /* BMI160_GYRO_HPP_ */

src/drivers/imu/bmi160/bmi160_main.cpp

@@ -1,7 +1,7 @@
 
 #include "bmi160.hpp"
 
-#include <board_config.h>
+#include <px4_config.h>
 #include <px4_getopt.h>
 
 /** driver 'main' command */
@@ -18,8 +18,6 @@ BMI160	*g_dev_ext; // on external bus
 
 void	start(bool, enum Rotation);
 void	stop(bool);
-void	test(bool);
-void	reset(bool);
 void	info(bool);
 void	regdump(bool);
 void	testerror(bool);
@@ -35,10 +33,7 @@ void	usage();
 void
 start(bool external_bus, enum Rotation rotation)
 {
-	int fd;
 	BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
-	const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
-	const char *path_gyro  = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
 
 	if (*g_dev_ptr != nullptr)
 		/* if already started, the still command succeeded */
@@ -49,14 +44,14 @@ start(bool external_bus, enum Rotation rotation)
 	/* create the driver */
 	if (external_bus) {
 #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
-		*g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, path_accel, path_gyro, PX4_SPIDEV_EXT_BMI, rotation);
+		*g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BMI, rotation);
 #else
 		errx(0, "External SPI not available");
 #endif
 
 	} else {
 #if defined(PX4_SPIDEV_BMI)
-		*g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, PX4_SPIDEV_BMI, rotation);
+		*g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BMI, rotation);
 #else
 		errx(0, "No Internal SPI CS");
 #endif
@@ -70,19 +65,6 @@ start(bool external_bus, enum Rotation rotation)
 		goto fail;
 	}
 
-	/* set the poll rate to default, starts automatic data collection */
-	fd = open(path_accel, O_RDONLY);
-
-	if (fd < 0) {
-		goto fail;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		goto fail;
-	}
-
-	close(fd);
-
 	exit(0);
 fail:
 
@@ -111,94 +93,6 @@ stop(bool external_bus)
 	exit(0);
 }
 
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-void
-test(bool external_bus)
-{
-	const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
-	const char *path_gyro  = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
-	sensor_accel_s a_report{};
-	sensor_gyro_s g_report{};
-	ssize_t sz;
-
-	/* get the driver */
-	int fd = open(path_accel, O_RDONLY);
-
-	if (fd < 0)
-		err(1, "%s open failed (try 'bmi160 start')",
-		    path_accel);
-
-	/* get the driver */
-	int fd_gyro = open(path_gyro, O_RDONLY);
-
-	if (fd_gyro < 0) {
-		err(1, "%s open failed", path_gyro);
-	}
-
-	/* do a simple demand read */
-	sz = read(fd, &a_report, sizeof(a_report));
-
-	if (sz != sizeof(a_report)) {
-		warnx("ret: %d, expected: %d", sz, sizeof(a_report));
-		err(1, "immediate acc read failed");
-	}
-
-	print_message(a_report);
-
-	/* do a simple demand read */
-	sz = read(fd_gyro, &g_report, sizeof(g_report));
-
-	if (sz != sizeof(g_report)) {
-		warnx("ret: %d, expected: %d", sz, sizeof(g_report));
-		err(1, "immediate gyro read failed");
-	}
-
-	print_message(g_report);
-
-	/* reset to default polling */
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		err(1, "reset to default polling");
-	}
-
-	close(fd);
-	close(fd_gyro);
-
-	/* XXX add poll-rate tests here too */
-
-	reset(external_bus);
-	errx(0, "PASS");
-}
-
-/**
- * Reset the driver.
- */
-void
-reset(bool external_bus)
-{
-	const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
-	int fd = open(path_accel, O_RDONLY);
-
-	if (fd < 0) {
-		err(1, "failed ");
-	}
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		err(1, "driver reset failed");
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		err(1, "driver poll restart failed");
-	}
-
-	close(fd);
-
-	exit(0);
-}
-
 /**
  * Print a little info about the driver.
  */
@@ -256,7 +150,7 @@ testerror(bool external_bus)
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'");
+	warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
@@ -307,20 +201,6 @@ bmi160_main(int argc, char *argv[])
 		bmi160::stop(external_bus);
 	}
 
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(verb, "test")) {
-		bmi160::test(external_bus);
-	}
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(verb, "reset")) {
-		bmi160::reset(external_bus);
-	}
-
 	/*
 	 * Print driver information.
 	 */