InvenSense ICM20948 move to PX4Accelerometer, PX4Gyroscope, PX4Magnetometer helpers

src/drivers/drv_sensor.h

@@ -61,6 +61,7 @@
 #define DRV_MAG_DEVTYPE_RM3100   0x07
 #define DRV_MAG_DEVTYPE_QMC5883  0x08
 #define DRV_MAG_DEVTYPE_AK09916  0x09
+#define DRV_DEVTYPE_ICM20948     0x0A
 #define DRV_ACC_DEVTYPE_LSM303D  0x11
 #define DRV_ACC_DEVTYPE_BMA180   0x12
 #define DRV_ACC_DEVTYPE_MPU6000  0x13

src/drivers/imu/icm20948/CMakeLists.txt

@@ -39,10 +39,8 @@ px4_add_module(
 		icm20948.cpp
 		icm20948_i2c.cpp
 		icm20948_spi.cpp
-		main.cpp
+		icm20948_main.cpp
-		accel.cpp
+		ICM20948_mag.cpp
-		gyro.cpp
-		mag.cpp
 		mag_i2c.cpp
 	DEPENDS
 	)

src/drivers/imu/icm20948/ICM20948_mag.cpp

@@ -40,56 +40,34 @@
  *
  */
 
-#include "mag.h"
+#include <px4_config.h>
-#include "icm20948.h"
+#include <px4_log.h>
+#include <px4_time.h>
+#include <lib/perf/perf_counter.h>
+#include <drivers/drv_hrt.h>
 
+#include "ICM20948_mag.h"
+#include "icm20948.h"
 
 // If interface is non-null, then it will used for interacting with the device.
 // Otherwise, it will passthrough the parent ICM20948
-ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, const char *path) :
+ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation) :
-	CDev("ICM20948_mag", path),
 	_interface(interface),
+	_px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH),
+		 rotation),
 	_parent(parent),
-	_mag_topic(nullptr),
+	_mag_reads(perf_alloc(PC_COUNT, "icm20948: mag_reads")),
-	_mag_orb_class_instance(-1),
+	_mag_errors(perf_alloc(PC_COUNT, "icm20948: mag_errors")),
-	_mag_class_instance(-1),
+	_mag_overruns(perf_alloc(PC_COUNT, "icm20948: mag_overruns")),
-	_mag_reading_data(false),
+	_mag_overflows(perf_alloc(PC_COUNT, "icm20948: mag_overflows")),
-	_mag_reports(nullptr),
+	_mag_duplicates(perf_alloc(PC_COUNT, "icm20948: mag_duplicates"))
-	_mag_scale{},
-	_mag_range_scale(),
-	_mag_reads(perf_alloc(PC_COUNT, "mpu9250_mag_reads")),
-	_mag_errors(perf_alloc(PC_COUNT, "mpu9250_mag_errors")),
-	_mag_overruns(perf_alloc(PC_COUNT, "mpu9250_mag_overruns")),
-	_mag_overflows(perf_alloc(PC_COUNT, "mpu9250_mag_overflows")),
-	_mag_duplicates(perf_alloc(PC_COUNT, "mpu9250_mag_duplicates")),
-	_mag_asa_x(1.0),
-	_mag_asa_y(1.0),
-	_mag_asa_z(1.0),
-	_last_mag_data{}
 {
-	// default mag scale factors
+	_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
-	_mag_scale.x_offset = 0;
+	_px4_mag.set_scale(MPU9250_MAG_RANGE_GA);
-	_mag_scale.x_scale  = 1.0f;
-	_mag_scale.y_offset = 0;
-	_mag_scale.y_scale  = 1.0f;
-	_mag_scale.z_offset = 0;
-	_mag_scale.z_scale  = 1.0f;
-
-	_mag_range_scale = MPU9250_MAG_RANGE_GA;
 }
 
 ICM20948_mag::~ICM20948_mag()
 {
-	if (_mag_class_instance != -1) {
-		unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance);
-	}
-
-	if (_mag_reports != nullptr) {
-		delete _mag_reports;
-	}
-
-	orb_unadvertise(_mag_topic);
-
 	perf_free(_mag_reads);
 	perf_free(_mag_errors);
 	perf_free(_mag_overruns);
@@ -97,43 +75,6 @@ ICM20948_mag::~ICM20948_mag()
 	perf_free(_mag_duplicates);
 }
 
-int
-ICM20948_mag::init()
-{
-	int ret = CDev::init();
-
-	/* if cdev init failed, bail now */
-	if (ret != OK) {
-		DEVICE_DEBUG("ICM20948 mag init failed");
-
-		return ret;
-	}
-
-	_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
-
-	if (_mag_reports == nullptr) {
-		return -ENOMEM;
-	}
-
-	_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
-
-
-	/* advertise sensor topic, measure manually to initialize valid report */
-	struct mag_report mrp;
-	_mag_reports->get(&mrp);
-
-	_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
-					 &_mag_orb_class_instance, (_parent->is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
-//    &_mag_orb_class_instance, ORB_PRIO_LOW);
-
-	if (_mag_topic == nullptr) {
-		PX4_ERR("ADVERT FAIL");
-		return -ENOMEM;
-	}
-
-	return OK;
-}
-
 bool ICM20948_mag::check_duplicate(uint8_t *mag_data)
 {
 	if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) {
@@ -149,27 +90,25 @@ bool ICM20948_mag::check_duplicate(uint8_t *mag_data)
 void
 ICM20948_mag::measure()
 {
-	uint8_t ret;
 	union raw_data_t {
 		struct ak8963_regs ak8963_data;
 		struct ak09916_regs ak09916_data;
 	} raw_data;
 
 
-	ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs));
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	int ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs));
 
 	if (ret == OK) {
 		raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2;
 
-		_measure(raw_data.ak8963_data);
+		_measure(timestamp_sample, raw_data.ak8963_data);
 	}
 }
 
 void
-ICM20948_mag::_measure(struct ak8963_regs data)
+ICM20948_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data)
 {
-	bool mag_notify = true;
-
 	if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
 		perf_count(_mag_duplicates);
 		return;
@@ -188,119 +127,14 @@ ICM20948_mag::_measure(struct ak8963_regs data)
 		perf_count(_mag_overflows);
 	}
 
-	mag_report	mrb;
+	_px4_mag.set_external(_parent->is_external());
-	mrb.timestamp = hrt_absolute_time();
+	_px4_mag.set_temperature(_parent->_last_temperature);
-//	mrb.is_external = false;
+	_px4_mag.set_error_count(perf_event_count(_mag_errors));
-
-	// need a better check here. Using _parent->is_external() for mpu9250 also sets the
-	// internal magnetometers connected to the "external" spi bus as external, at least
-	// on Pixhawk 2.1. For now assuming the ICM20948 is only used on Here GPS, hence external.
-	if (_parent->_whoami == ICM_WHOAMI_20948) {
-		mrb.is_external = _parent->is_external();
-
-	} else {
-		mrb.is_external = false;
-	}
 
 	/*
-	 * Align axes - note the accel & gryo are also re-aligned so this
+	 * Align axes - Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them.
-	 *              doesn't look obvious with the datasheet
 	 */
-	float xraw_f, yraw_f, zraw_f;
+	_px4_mag.update(timestamp_sample, data.y, data.x, -data.z);
-
-	if (_parent->_whoami == ICM_WHOAMI_20948) {
-		/*
-		 * Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them.
-		 */
-		mrb.x_raw = data.y;
-		mrb.y_raw = data.x;
-		mrb.z_raw = -data.z;
-
-		xraw_f = data.y;
-		yraw_f = data.x;
-		zraw_f = -data.z;
-
-	} else {
-		mrb.x_raw =  data.x;
-		mrb.y_raw = -data.y;
-		mrb.z_raw = -data.z;
-
-		xraw_f =  data.x;
-		yraw_f = -data.y;
-		zraw_f = -data.z;
-	}
-
-	/* apply user specified rotation */
-	rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f);
-
-	if (_parent->_whoami == ICM_WHOAMI_20948) {
-		rotate_3f(ROTATION_YAW_270, xraw_f, yraw_f, zraw_f); //offset between accel/gyro and mag on icm20948
-	}
-
-	mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale;
-	mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale;
-	mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale;
-	mrb.scaling = _mag_range_scale;
-	mrb.temperature = _parent->_last_temperature;
-	mrb.device_id = _parent->_mag->_device_id.devid;
-
-	mrb.error_count = perf_event_count(_mag_errors);
-
-	_mag_reports->force(&mrb);
-
-	/* notify anyone waiting for data */
-	if (mag_notify) {
-		poll_notify(POLLIN);
-	}
-
-	if (mag_notify && !(_pub_blocked)) {
-		/* publish it */
-		orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb);
-	}
-}
-
-int
-ICM20948_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	/*
-	 * Repeated in ICM20948_accel::ioctl
-	 * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500
-	 */
-
-	switch (cmd) {
-
-	case SENSORIOCRESET:
-		return ak8963_reset();
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-			/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-			case SENSOR_POLLRATE_DEFAULT:
-				return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE);
-
-			/* adjust to a legal polling interval in Hz */
-			default:
-				return _parent->_set_pollrate(arg);
-			}
-		}
-
-	case MAGIOCSSCALE:
-		/* copy scale in */
-		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
-		return OK;
-
-	case MAGIOCGSCALE:
-		/* copy scale out */
-		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
-		return OK;
-
-	default:
-		return (int)CDev::ioctl(filp, cmd, arg);
-	}
 }
 
 void
@@ -341,7 +175,7 @@ ICM20948_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
 uint8_t
 ICM20948_mag::read_reg(unsigned int reg)
 {
-	uint8_t buf;
+	uint8_t buf{};
 
 	if (_interface == nullptr) {
 		passthrough_read(reg, &buf, 0x01);
@@ -428,9 +262,7 @@ ICM20948_mag::ak8963_read_adjustments(void)
 		}
 	}
 
-	_mag_asa_x = ak8963_ASA[0];
+	_px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);
-	_mag_asa_y = ak8963_ASA[1];
-	_mag_asa_z = ak8963_ASA[2];
 
 	return true;
 }

src/drivers/imu/icm20948/ICM20948_mag.h

@@ -33,16 +33,12 @@
 
 #pragma once
 
-#include <drivers/device/i2c.h>
+#include <perf/perf_counter.h>
-#include <drivers/device/ringbuffer.h>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <drivers/drv_device.h>
+#include <drivers/device/Device.hpp>
-#include <drivers/drv_mag.h>
-#include <lib/perf/perf_counter.h>
-#include <uORB/uORB.h>
 
 /* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
-
+static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f};
-#define MPU9250_MAG_RANGE_GA        1.5e-3f;
 
 /* we are using the continuous fixed sampling rate of 100Hz */
 
@@ -132,15 +128,12 @@ typedef device::Device *(*ICM20948_mag_constructor)(int, bool);
 /**
  * Helper class implementing the magnetometer driver node.
  */
-class ICM20948_mag : public device::CDev
+class ICM20948_mag
 {
 public:
-	ICM20948_mag(ICM20948 *parent, device::Device *interface, const char *path);
+	ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation);
 	~ICM20948_mag();
 
-	virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
-	virtual int init();
-
 	void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = NULL);
 	void passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size);
 	void passthrough_write(uint8_t reg, uint8_t val);
@@ -152,8 +145,10 @@ public:
 	bool ak8963_check_id(uint8_t &id);
 	bool ak8963_read_adjustments(void);
 
+	void print_status() { _px4_mag.print_status(); }
+
 protected:
-	Device			*_interface;
+	device::Device			*_interface;
 
 	friend class ICM20948;
 
@@ -161,7 +156,7 @@ protected:
 	void measure();
 
 	/* Update the state with prefetched data (internally called by the regular measure() )*/
-	void _measure(struct ak8963_regs data);
+	void _measure(hrt_abstime timestamp, struct ak8963_regs data);
 
 	uint8_t read_reg(unsigned reg);
 	void write_reg(unsigned reg, uint8_t value);
@@ -169,27 +164,23 @@ protected:
 	bool is_passthrough() { return _interface == nullptr; }
 
 private:
+
+	PX4Magnetometer		_px4_mag;
+
 	ICM20948 *_parent;
-	orb_advert_t _mag_topic;
+
-	int _mag_orb_class_instance;
+	bool _mag_reading_data{false};
-	int _mag_class_instance;
+
-	bool _mag_reading_data;
-	ringbuffer::RingBuffer *_mag_reports;
-	struct mag_calibration_s _mag_scale;
-	float _mag_range_scale;
 	perf_counter_t _mag_reads;
 	perf_counter_t _mag_errors;
 	perf_counter_t _mag_overruns;
 	perf_counter_t _mag_overflows;
 	perf_counter_t _mag_duplicates;
-	float _mag_asa_x;
-	float _mag_asa_y;
-	float _mag_asa_z;
 
 	bool check_duplicate(uint8_t *mag_data);
 
 	// keep last mag reading for duplicate detection
-	uint8_t			_last_mag_data[6];
+	uint8_t			_last_mag_data[6] {};
 
 	/* do not allow to copy this class due to pointer data members */
 	ICM20948_mag(const ICM20948_mag &);

src/drivers/imu/icm20948/accel.cpp

@@ -1,123 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file gyro.cpp
- *
- * Driver for the Invensense mpu9250 connected via SPI.
- *
- * @author Andrew Tridgell
- *
- * based on the mpu6000 driver
- */
-
-#include <drivers/device/spi.h>
-
-#include "accel.h"
-#include "icm20948.h"
-
-ICM20948_accel::ICM20948_accel(ICM20948 *parent, const char *path) :
-	CDev("ICM20948_accel", path),
-	_parent(parent)
-{
-}
-
-ICM20948_accel::~ICM20948_accel()
-{
-	if (_accel_class_instance != -1) {
-		unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
-	}
-}
-
-int
-ICM20948_accel::init()
-{
-	// do base class init
-	int ret = CDev::init();
-
-	/* if probe/setup failed, bail now */
-	if (ret != OK) {
-		DEVICE_DEBUG("accel init failed");
-		return ret;
-	}
-
-	_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
-
-	return ret;
-}
-
-void
-ICM20948_accel::parent_poll_notify()
-{
-	poll_notify(POLLIN);
-}
-
-int
-ICM20948_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	/*
-	 * Repeated in ICM20948_mag::ioctl
-	 * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500
-	 */
-
-	switch (cmd) {
-	case SENSORIOCRESET: {
-			return _parent->reset();
-		}
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-			/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-			case SENSOR_POLLRATE_DEFAULT:
-				return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE);
-
-			/* adjust to a legal polling interval in Hz */
-			default:
-				return _parent->_set_pollrate(arg);
-			}
-		}
-
-	case ACCELIOCSSCALE: {
-			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
-			memcpy(&_parent->_accel_scale, s, sizeof(_parent->_accel_scale));
-			return OK;
-		}
-
-	default:
-		return CDev::ioctl(filp, cmd, arg);
-	}
-}

src/drivers/imu/icm20948/accel.h

@@ -1,67 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <drivers/device/device.h>
-#include <drivers/drv_accel.h>
-#include <uORB/uORB.h>
-
-class ICM20948;
-
-/**
- * Helper class implementing the accel driver node.
- */
-class ICM20948_accel : public device::CDev
-{
-public:
-	ICM20948_accel(ICM20948 *parent, const char *path);
-	~ICM20948_accel();
-
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	virtual int		init();
-
-protected:
-	friend class ICM20948;
-
-	void			parent_poll_notify();
-
-private:
-	ICM20948			*_parent;
-
-	orb_advert_t		_accel_topic{nullptr};
-	int			_accel_orb_class_instance{-1};
-	int			_accel_class_instance{-1};
-
-};

src/drivers/imu/icm20948/gyro.cpp

@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file gyro.cpp
- *
- * Driver for the Invensense icm20948 connected via SPI.
- *
- *
- * based on the mpu9250 driver
- */
-
-#include "gyro.h"
-#include "icm20948.h"
-
-ICM20948_gyro::ICM20948_gyro(ICM20948 *parent, const char *path) :
-	CDev("ICM20948_gyro", path),
-	_parent(parent)
-{
-}
-
-ICM20948_gyro::~ICM20948_gyro()
-{
-	if (_gyro_class_instance != -1) {
-		unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
-	}
-}
-
-int
-ICM20948_gyro::init()
-{
-	// do base class init
-	int 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
-ICM20948_gyro::parent_poll_notify()
-{
-	poll_notify(POLLIN);
-}
-
-int
-ICM20948_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	/* these are shared with the accel side */
-	case SENSORIOCSPOLLRATE:
-	case SENSORIOCRESET:
-		return _parent->_accel->ioctl(filp, cmd, arg);
-
-	case GYROIOCSSCALE:
-		/* copy scale in */
-		memcpy(&_parent->_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_parent->_gyro_scale));
-		return OK;
-
-	default:
-		return CDev::ioctl(filp, cmd, arg);
-	}
-}

src/drivers/imu/icm20948/gyro.h

@@ -1,66 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <drivers/device/device.h>
-#include <drivers/drv_gyro.h>
-#include <uORB/uORB.h>
-
-class ICM20948;
-
-/**
- * Helper class implementing the gyro driver node.
- */
-class ICM20948_gyro : public device::CDev
-{
-public:
-	ICM20948_gyro(ICM20948 *parent, const char *path);
-	~ICM20948_gyro();
-
-	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
-
-	virtual int		init();
-
-protected:
-	friend class ICM20948;
-
-	void			parent_poll_notify();
-
-private:
-	ICM20948			*_parent;
-
-	orb_advert_t		_gyro_topic{nullptr};
-	int			_gyro_orb_class_instance{-1};
-	int			_gyro_class_instance{-1};
-};

src/drivers/imu/icm20948/icm20948.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,9 +36,21 @@
  *
  * Driver for the Invensense ICM20948 connected via I2C or SPI.
  *
+ *
  * based on the mpu9250 driver
  */
 
+#include <px4_config.h>
+#include <px4_time.h>
+#include <lib/ecl/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <systemlib/conversions.h>
+#include <systemlib/px4_macros.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <lib/conversion/rotation.h>
+
+#include "ICM20948_mag.h"
 #include "icm20948.h"
 
 /*
@@ -55,149 +67,54 @@
   list of registers that will be checked in check_registers(). Note
   that MPUREG_PRODUCT_ID must be first in the list.
  */
-const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = {
+const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { ICMREG_20948_USER_CTRL,
-	ICMREG_20948_USER_CTRL,
+											 ICMREG_20948_PWR_MGMT_1,
-	ICMREG_20948_PWR_MGMT_1,
+											 ICMREG_20948_PWR_MGMT_2,
-	ICMREG_20948_PWR_MGMT_2,
+											 ICMREG_20948_INT_PIN_CFG,
-	ICMREG_20948_INT_PIN_CFG,
+											 ICMREG_20948_INT_ENABLE,
-	ICMREG_20948_INT_ENABLE,
+											 ICMREG_20948_INT_ENABLE_1,
-	ICMREG_20948_INT_ENABLE_1,
+											 ICMREG_20948_INT_ENABLE_2,
-	ICMREG_20948_INT_ENABLE_2,
+											 ICMREG_20948_INT_ENABLE_3,
-	ICMREG_20948_INT_ENABLE_3,
+											 ICMREG_20948_GYRO_SMPLRT_DIV,
-	ICMREG_20948_GYRO_SMPLRT_DIV,
+											 ICMREG_20948_GYRO_CONFIG_1,
-	ICMREG_20948_GYRO_CONFIG_1,
+											 ICMREG_20948_GYRO_CONFIG_2,
-	ICMREG_20948_GYRO_CONFIG_2,
+											 ICMREG_20948_ACCEL_SMPLRT_DIV_1,
-	ICMREG_20948_ACCEL_SMPLRT_DIV_1,
+											 ICMREG_20948_ACCEL_SMPLRT_DIV_2,
-	ICMREG_20948_ACCEL_SMPLRT_DIV_2,
+											 ICMREG_20948_ACCEL_CONFIG,
-	ICMREG_20948_ACCEL_CONFIG,
+											 ICMREG_20948_ACCEL_CONFIG_2
-	ICMREG_20948_ACCEL_CONFIG_2
+										       };
-};
 
-
+ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation,
-ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel,
-		   const char *path_gyro, const char *path_mag,
-		   enum Rotation rotation,
 		   bool magnetometer_only) :
 	ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
 	_interface(interface),
-	_whoami(0),
+	_px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
-	_accel(magnetometer_only ? nullptr : new ICM20948_accel(this, path_accel)),
+	_px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
-	_gyro(magnetometer_only ? nullptr : new ICM20948_gyro(this, path_gyro)),
+	_mag(this, mag_interface, rotation),
-	_mag(new ICM20948_mag(this, mag_interface, path_mag)),
 	_selected_bank(0xFF),	// invalid/improbable bank value, will be set on first read/write
 	_magnetometer_only(magnetometer_only),
-	_call_interval(0),
-	_accel_reports(nullptr),
-	_accel_scale{},
-	_accel_range_scale(0.0f),
-	_accel_range_m_s2(0.0f),
-	_accel_topic(nullptr),
-	_gyro_reports(nullptr),
-	_gyro_scale{},
-	_gyro_range_scale(0.0f),
-	_gyro_range_rad_s(0.0f),
 	_dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
 	_dlpf_freq_icm_gyro(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
 	_dlpf_freq_icm_accel(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
-	_sample_rate(1000),
+	_accel_reads(perf_alloc(PC_COUNT, "icm20948: acc_read")),
-	_accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")),
+	_gyro_reads(perf_alloc(PC_COUNT, "icm20948: gyro_read")),
-	_gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")),
+	_sample_perf(perf_alloc(PC_ELAPSED, "icm20948: read")),
-	_sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")),
+	_bad_transfers(perf_alloc(PC_COUNT, "icm20948: bad_trans")),
-	_bad_transfers(perf_alloc(PC_COUNT, "mpu9250_bad_trans")),
+	_bad_registers(perf_alloc(PC_COUNT, "icm20948: bad_reg")),
-	_bad_registers(perf_alloc(PC_COUNT, "mpu9250_bad_reg")),
+	_good_transfers(perf_alloc(PC_COUNT, "icm20948: good_trans")),
-	_good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_trans")),
+	_reset_retries(perf_alloc(PC_COUNT, "icm20948: reset")),
-	_reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset")),
+	_duplicates(perf_alloc(PC_COUNT, "icm20948: dupe"))
-	_duplicates(perf_alloc(PC_COUNT, "mpu9250_dupe")),
-	_register_wait(0),
-	_reset_wait(0),
-	_accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
-	_accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
-	_accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
-	_gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
-	_gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
-	_gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
-	_accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE),
-	_gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true),
-	_rotation(rotation),
-	_checked_registers(nullptr),
-	_checked_next(0),
-	_num_checked_registers(0),
-	_last_temperature(0),
-	_last_accel_data{},
-	_got_duplicate(false)
 {
-	if (_accel != nullptr) {
+	_px4_accel.set_device_type(DRV_DEVTYPE_ICM20948);
-		/* Set device parameters and make sure parameters of the bus device are adopted */
+	_px4_gyro.set_device_type(DRV_DEVTYPE_ICM20948);
-		_accel->_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
-		_accel->_device_id.devid_s.bus_type = (device::Device::DeviceBusType)_interface->get_device_bus_type();
-		_accel->_device_id.devid_s.bus = _interface->get_device_bus();
-		_accel->_device_id.devid_s.address = _interface->get_device_address();
-	}
-
-	if (_gyro != nullptr) {
-		/* Prime _gyro with common devid. */
-		/* Set device parameters and make sure parameters of the bus device are adopted */
-		_gyro->_device_id.devid = 0;
-		_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250;
-		_gyro->_device_id.devid_s.bus_type = _interface->get_device_bus_type();
-		_gyro->_device_id.devid_s.bus = _interface->get_device_bus();
-		_gyro->_device_id.devid_s.address = _interface->get_device_address();
-	}
-
-	/* Prime _mag with common devid. */
-	_mag->_device_id.devid = 0;
-	_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
-	_mag->_device_id.devid_s.bus_type = _interface->get_device_bus_type();
-	_mag->_device_id.devid_s.bus = _interface->get_device_bus();
-	_mag->_device_id.devid_s.address = _interface->get_device_address();
-
-	// default accel scale factors
-	_accel_scale.x_offset = 0;
-	_accel_scale.x_scale  = 1.0f;
-	_accel_scale.y_offset = 0;
-	_accel_scale.y_scale  = 1.0f;
-	_accel_scale.z_offset = 0;
-	_accel_scale.z_scale  = 1.0f;
-
-	// default gyro scale factors
-	_gyro_scale.x_offset = 0;
-	_gyro_scale.x_scale  = 1.0f;
-	_gyro_scale.y_offset = 0;
-	_gyro_scale.y_scale  = 1.0f;
-	_gyro_scale.z_offset = 0;
-	_gyro_scale.z_scale  = 1.0f;
 }
 
 ICM20948::~ICM20948()
 {
-	/* make sure we are truly inactive */
+	// make sure we are truly inactive
 	stop();
-	_call_interval = 0;
 
-	if (!_magnetometer_only) {
+	// delete the perf counter
-		orb_unadvertise(_accel_topic);
-		orb_unadvertise(_gyro->_gyro_topic);
-	}
-
-	/* delete the accel subdriver */
-	delete _accel;
-
-	/* delete the gyro subdriver */
-	delete _gyro;
-
-	/* delete the magnetometer subdriver */
-	delete _mag;
-
-	/* free any existing reports */
-	if (_accel_reports != nullptr) {
-		delete _accel_reports;
-	}
-
-	if (_gyro_reports != nullptr) {
-		delete _gyro_reports;
-	}
-
-	/* delete the perf counter */
 	perf_free(_sample_perf);
 	perf_free(_accel_reads);
 	perf_free(_gyro_reads);
@@ -211,8 +128,6 @@ ICM20948::~ICM20948()
 int
 ICM20948::init()
 {
-	irqstate_t state;
-
 	/*
 	 * If the MPU is using I2C we should reduce the sample rate to 200Hz and
 	 * make the integration autoreset faster so that we integrate just one
@@ -222,8 +137,6 @@ ICM20948::init()
 
 	if (is_i2c && !_magnetometer_only) {
 		_sample_rate = 200;
-		_accel_int.set_autoreset_interval(1000000 / 1000);
-		_gyro_int.set_autoreset_interval(1000000 / 1000);
 	}
 
 	int ret = probe();
@@ -233,157 +146,38 @@ ICM20948::init()
 		return ret;
 	}
 
-	state = px4_enter_critical_section();
 	_reset_wait = hrt_absolute_time() + 100000;
-	px4_leave_critical_section(state);
 
 	if (reset_mpu() != OK) {
 		PX4_ERR("Exiting! Device failed to take initialization");
 		return ret;
 	}
 
-	if (!_magnetometer_only) {
-		/* allocate basic report buffers */
-		_accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
-		ret = -ENOMEM;
-
-		if (_accel_reports == nullptr) {
-			return ret;
-		}
-
-		_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
-
-		if (_gyro_reports == nullptr) {
-			return ret;
-		}
-
-		/* Initialize offsets and scales */
-		_accel_scale.x_offset = 0;
-		_accel_scale.x_scale  = 1.0f;
-		_accel_scale.y_offset = 0;
-		_accel_scale.y_scale  = 1.0f;
-		_accel_scale.z_offset = 0;
-		_accel_scale.z_scale  = 1.0f;
-
-		_gyro_scale.x_offset = 0;
-		_gyro_scale.x_scale  = 1.0f;
-		_gyro_scale.y_offset = 0;
-		_gyro_scale.y_scale  = 1.0f;
-		_gyro_scale.z_offset = 0;
-		_gyro_scale.z_scale  = 1.0f;
-
-		// set software low pass filter for controllers
-		param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
-		float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
-
-		if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) {
-			PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut));
-
-			_accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
-			_accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
-			_accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut);
-
-		}
-
-		param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
-		float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
-
-		if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) {
-			PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut));
-
-			_gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
-			_gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
-			_gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut);
-
-		}
-
-		/* do CDev init for the accel device node */
-		ret = _accel->init();
-
-		/* if probe/setup failed, bail now */
-		if (ret != OK) {
-			PX4_DEBUG("accel init failed");
-			return ret;
-		}
-
-		/* do CDev init for the gyro device node */
-		ret = _gyro->init();
-
-		/* if probe/setup failed, bail now */
-		if (ret != OK) {
-			PX4_DEBUG("gyro init failed");
-			return ret;
-		}
-	}
-
 	/* Magnetometer setup */
 
 #ifdef USE_I2C
-	up_udelay(100);
+	px4_usleep(100);
 
-	if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) {
+	if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) {
 		PX4_ERR("failed to setup ak8963 interface");
 	}
 
 #endif /* USE_I2C */
 
-	/* do CDev init for the mag device node */
+	ret = _mag.ak8963_reset();
-	ret = _mag->init();
-
-	/* if probe/setup failed, bail now */
-	if (ret != OK) {
-		PX4_DEBUG("mag init failed");
-		return ret;
-	}
-
-	ret = _mag->ak8963_reset();
 
 	if (ret != OK) {
 		PX4_DEBUG("mag reset failed");
 		return ret;
 	}
 
-	measure();
+	start();
-
-	if (!_magnetometer_only) {
-		/* advertise sensor topic, measure manually to initialize valid report */
-		sensor_accel_s arp;
-		_accel_reports->get(&arp);
-
-		/* measurement will have generated a report, publish */
-		_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
-						   &_accel->_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
-
-		if (_accel_topic == nullptr) {
-			PX4_ERR("ADVERT FAIL");
-			return ret;
-		}
-
-		/* advertise sensor topic, measure manually to initialize valid report */
-		sensor_gyro_s grp;
-		_gyro_reports->get(&grp);
-
-		_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
-				     &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
-
-		if (_gyro->_gyro_topic == nullptr) {
-			PX4_ERR("ADVERT FAIL");
-			return ret;
-		}
-	}
 
 	return ret;
 }
 
-uint8_t ICM20948::get_whoami()
-{
-	return _whoami;
-}
-
 int ICM20948::reset()
 {
-	irqstate_t state;
-
 	/* When the mpu9250 starts from 0V the internal power on circuit
 	 * per the data sheet will require:
 	 *
@@ -394,21 +188,15 @@ int ICM20948::reset()
 	px4_usleep(110000);
 
 	// Hold off sampling until done (100 MS will be shortened)
-	state = px4_enter_critical_section();
 	_reset_wait = hrt_absolute_time() + 100000;
-	px4_leave_critical_section(state);
 
-	int ret;
+	int ret = reset_mpu();
-
-	ret = reset_mpu();
 
 	if (ret == OK && (_whoami == ICM_WHOAMI_20948)) {
-		ret = _mag->ak8963_reset();
+		ret = _mag.ak8963_reset();
 	}
 
-	state = px4_enter_critical_section();
 	_reset_wait = hrt_absolute_time() + 10;
-	px4_leave_critical_section(state);
 
 	return ret;
 }
@@ -450,17 +238,15 @@ int ICM20948::reset_mpu()
 	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
 	// scaling factor:
 	// 1/(2^15)*(2000/180)*PI
-	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
+	_px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
-	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
 
 	set_accel_range(ACCEL_RANGE_G);
 
 	// INT CFG => Interrupt on Data Ready
-	write_checked_reg(MPU_OR_ICM(MPUREG_INT_ENABLE, ICMREG_20948_INT_ENABLE_1),
+	write_checked_reg(ICMREG_20948_INT_ENABLE_1, BIT_RAW_RDY_EN);        // INT: Raw data ready
-			  BIT_RAW_RDY_EN);        // INT: Raw data ready
 
 #ifdef USE_I2C
-	bool bypass = !_mag->is_passthrough();
+	bool bypass = !_mag.is_passthrough();
 #else
 	bool bypass = false;
 #endif
@@ -474,11 +260,9 @@ int ICM20948::reset_mpu()
 	 * so bypass is true if the mag has an i2c non null interfaces.
 	 */
 
-	write_checked_reg(MPU_OR_ICM(MPUREG_INT_PIN_CFG, ICMREG_20948_INT_PIN_CFG),
+	write_checked_reg(ICMREG_20948_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
-			  BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
 
-	write_checked_reg(MPU_OR_ICM(MPUREG_ACCEL_CONFIG2, ICMREG_20948_ACCEL_CONFIG_2),
+	write_checked_reg(ICMREG_20948_ACCEL_CONFIG_2, ICM_BITS_DEC3_CFG_32);
-			  MPU_OR_ICM(BITS_ACCEL_CONFIG2_41HZ, ICM_BITS_DEC3_CFG_32));
 
 	retries = 3;
 	bool all_ok = false;
@@ -573,53 +357,6 @@ ICM20948::_set_sample_rate(unsigned desired_sample_rate_hz)
 	}
 }
 
-/*
- * Set poll rate
- */
-int
-ICM20948::_set_pollrate(unsigned long rate)
-{
-	if (rate == 0) {
-		return -EINVAL;
-
-	} else {
-		/* do we need to start internal polling? */
-		bool want_start = (_call_interval == 0);
-
-		/* convert hz to hrt interval via microseconds */
-		unsigned interval = 1000000 / rate;
-
-		/* check against maximum sane rate */
-		if (interval < 1000) {
-			return -EINVAL;
-		}
-
-		// adjust filters
-		float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
-		float sample_rate = 1.0e6f / interval;
-		_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-		_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-		_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
-
-
-		float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
-		_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
-		_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
-		_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
-
-		/* update interval for next measurement */
-		/* XXX this is a bit shady, but no other way to adjust... */
-		_call_interval = interval;
-
-		/* if we need to start the poll state machine, do it */
-		if (want_start) {
-			start();
-		}
-
-		return OK;
-	}
-}
-
 /*
   set the DLPF filter frequency. This affects both accel and gyro.
  */
@@ -635,39 +372,39 @@ ICM20948::_set_dlpf_filter(uint16_t frequency_hz)
 		   choose next highest filter frequency available for gyroscope
 		 */
 		if (frequency_hz == 0) {
-			_dlpf_freq_icm_gyro = 0;
+			//_dlpf_freq_icm_gyro = 0;
 			filter = ICM_BITS_GYRO_DLPF_CFG_361HZ;
 
 		} else if (frequency_hz <= 5) {
-			_dlpf_freq_icm_gyro = 5;
+			//_dlpf_freq_icm_gyro = 5;
 			filter = ICM_BITS_GYRO_DLPF_CFG_5HZ;
 
 		} else if (frequency_hz <= 11) {
-			_dlpf_freq_icm_gyro = 11;
+			//_dlpf_freq_icm_gyro = 11;
 			filter = ICM_BITS_GYRO_DLPF_CFG_11HZ;
 
 		} else if (frequency_hz <= 23) {
-			_dlpf_freq_icm_gyro = 23;
+			//_dlpf_freq_icm_gyro = 23;
 			filter = ICM_BITS_GYRO_DLPF_CFG_23HZ;
 
 		} else if (frequency_hz <= 51) {
-			_dlpf_freq_icm_gyro = 51;
+			//_dlpf_freq_icm_gyro = 51;
 			filter = ICM_BITS_GYRO_DLPF_CFG_51HZ;
 
 		} else if (frequency_hz <= 119) {
-			_dlpf_freq_icm_gyro = 119;
+			//_dlpf_freq_icm_gyro = 119;
 			filter = ICM_BITS_GYRO_DLPF_CFG_119HZ;
 
 		} else if (frequency_hz <= 151) {
-			_dlpf_freq_icm_gyro = 151;
+			//_dlpf_freq_icm_gyro = 151;
 			filter = ICM_BITS_GYRO_DLPF_CFG_151HZ;
 
 		} else if (frequency_hz <= 197) {
-			_dlpf_freq_icm_gyro = 197;
+			//_dlpf_freq_icm_gyro = 197;
 			filter = ICM_BITS_GYRO_DLPF_CFG_197HZ;
 
 		} else {
-			_dlpf_freq_icm_gyro = 0;
+			//_dlpf_freq_icm_gyro = 0;
 			filter = ICM_BITS_GYRO_DLPF_CFG_361HZ;
 		}
 
@@ -677,35 +414,35 @@ ICM20948::_set_dlpf_filter(uint16_t frequency_hz)
 		   choose next highest filter frequency available for accelerometer
 		 */
 		if (frequency_hz == 0) {
-			_dlpf_freq_icm_accel = 0;
+			//_dlpf_freq_icm_accel = 0;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ;
 
 		} else if (frequency_hz <= 5) {
-			_dlpf_freq_icm_accel = 5;
+			//_dlpf_freq_icm_accel = 5;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ;
 
 		} else if (frequency_hz <= 11) {
-			_dlpf_freq_icm_accel = 11;
+			//_dlpf_freq_icm_accel = 11;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ;
 
 		} else if (frequency_hz <= 23) {
-			_dlpf_freq_icm_accel = 23;
+			//_dlpf_freq_icm_accel = 23;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ;
 
 		} else if (frequency_hz <= 50) {
-			_dlpf_freq_icm_accel = 50;
+			//_dlpf_freq_icm_accel = 50;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ;
 
 		} else if (frequency_hz <= 111) {
-			_dlpf_freq_icm_accel = 111;
+			//_dlpf_freq_icm_accel = 111;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ;
 
 		} else if (frequency_hz <= 246) {
-			_dlpf_freq_icm_accel = 246;
+			//_dlpf_freq_icm_accel = 246;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ;
 
 		} else {
-			_dlpf_freq_icm_accel = 0;
+			//_dlpf_freq_icm_accel = 0;
 			filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ;
 		}
 
@@ -764,15 +501,10 @@ ICM20948::select_register_bank(uint8_t bank)
 uint8_t
 ICM20948::read_reg(unsigned reg, uint32_t speed)
 {
-	uint8_t buf;
+	uint8_t buf{};
 
-	if (_whoami == ICM_WHOAMI_20948) {
+	select_register_bank(REG_BANK(reg));
-		select_register_bank(REG_BANK(reg));
+	_interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1);
-		_interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1);
-
-	} else {
-		_interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1);
-	}
 
 	return buf;
 }
@@ -780,35 +512,22 @@ ICM20948::read_reg(unsigned reg, uint32_t speed)
 uint8_t
 ICM20948::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count)
 {
-	uint8_t ret;
+	if (buf == NULL) {
-
+		return PX4_ERROR;
-	if (buf == NULL) { return PX4_ERROR; }
-
-	if (_whoami == ICM_WHOAMI_20948) {
-		select_register_bank(REG_BANK(start_reg));
-		ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count);
-
-	} else {
-		ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count);
 	}
 
-	return ret;
+	select_register_bank(REG_BANK(start_reg));
+	return _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count);
 }
 
 uint16_t
 ICM20948::read_reg16(unsigned reg)
 {
-	uint8_t buf[2];
+	uint8_t buf[2] {};
 
 	// general register transfer at low clock speed
-
+	select_register_bank(REG_BANK(reg));
-	if (_whoami == ICM_WHOAMI_20948) {
+	_interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf));
-		select_register_bank(REG_BANK(reg));
-		_interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf));
-
-	} else {
-		_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
-	}
 
 	return (uint16_t)(buf[0] << 8) | buf[1];
 }
@@ -817,22 +536,14 @@ void
 ICM20948::write_reg(unsigned reg, uint8_t value)
 {
 	// general register transfer at low clock speed
-
+	select_register_bank(REG_BANK(reg));
-	if (_whoami == ICM_WHOAMI_20948) {
+	_interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1);
-		select_register_bank(REG_BANK(reg));
-		_interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1);
-
-	} else {
-		_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
-	}
 }
 
 void
 ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 {
-	uint8_t	val;
+	uint8_t	val = read_reg(reg);
-
-	val = read_reg(reg);
 	val &= ~clearbits;
 	val |= setbits;
 	write_reg(reg, val);
@@ -841,9 +552,7 @@ ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 void
 ICM20948::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
 {
-	uint8_t	val;
+	uint8_t	val = read_reg(reg);
-
-	val = read_reg(reg);
 	val &= ~clearbits;
 	val |= setbits;
 	write_checked_reg(reg, val);
@@ -868,27 +577,27 @@ ICM20948::set_accel_range(unsigned max_g_in)
 {
 	uint8_t afs_sel;
 	float lsb_per_g;
-	float max_accel_g;
+	//float max_accel_g;
 
 	if (max_g_in > 8) { // 16g - AFS_SEL = 3
 		afs_sel = 3;
 		lsb_per_g = 2048;
-		max_accel_g = 16;
+		//max_accel_g = 16;
 
 	} else if (max_g_in > 4) { //  8g - AFS_SEL = 2
 		afs_sel = 2;
 		lsb_per_g = 4096;
-		max_accel_g = 8;
+		//max_accel_g = 8;
 
 	} else if (max_g_in > 2) { //  4g - AFS_SEL = 1
 		afs_sel = 1;
 		lsb_per_g = 8192;
-		max_accel_g = 4;
+		//max_accel_g = 4;
 
 	} else {                //  2g - AFS_SEL = 0
 		afs_sel = 0;
 		lsb_per_g = 16384;
-		max_accel_g = 2;
+		//max_accel_g = 2;
 	}
 
 	switch (_whoami) {
@@ -897,8 +606,7 @@ ICM20948::set_accel_range(unsigned max_g_in)
 		break;
 	}
 
-	_accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g);
+	_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
-	_accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G;
 
 	return OK;
 }
@@ -909,15 +617,7 @@ ICM20948::start()
 	/* make sure we are stopped first */
 	stop();
 
-	/* discard any stale data in the buffers */
+	ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000);
-	if (!_magnetometer_only) {
-		_accel_reports->flush();
-		_gyro_reports->flush();
-	}
-
-	_mag->_mag_reports->flush();
-
-	ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 10000);
 }
 
 void
@@ -968,12 +668,7 @@ ICM20948::check_registers(void)
 			// if the product_id is wrong then reset the
 			// sensor completely
 
-			if (_whoami == ICM_WHOAMI_20948) {
+			reset_mpu();
-				// reset_mpu();
-			} else {
-				write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
-				write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO);
-			}
 
 			// after doing a reset we need to wait a long
 			// time before we do any other register writes
@@ -1044,7 +739,6 @@ bool ICM20948::check_duplicate(uint8_t *accel_data)
 void
 ICM20948::measure()
 {
-
 	if (hrt_absolute_time() < _reset_wait) {
 		// we're waiting for a reset to complete
 		return;
@@ -1067,11 +761,12 @@ ICM20948::measure()
 	/* start measuring */
 	perf_begin(_sample_perf);
 
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
 	/*
 	 * Fetch the full set of measurements from the MPU9250 in one pass
 	 */
 
-	if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) {
+	if ((!_magnetometer_only || _mag.is_passthrough()) && _register_wait == 0) {
 
 		select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H));
 
@@ -1095,15 +790,15 @@ ICM20948::measure()
 
 #   ifdef USE_I2C
 
-	if (_mag->is_passthrough()) {
+	if (_mag.is_passthrough()) {
 #   endif
 
-		_mag->_measure(mpu_report.mag);
+		_mag._measure(timestamp_sample, mpu_report.mag);
 
 #   ifdef USE_I2C
 
 	} else {
-		_mag->measure();
+		_mag.measure();
 	}
 
 #   endif
@@ -1155,6 +850,8 @@ ICM20948::measure()
 	 */
 	_last_temperature = (report.temp) / 333.87f + 21.0f;
 
+	_px4_accel.set_temperature(_last_temperature);
+	_px4_gyro.set_temperature(_last_temperature);
 
 	/*
 	 * Convert and publish accelerometer and gyrometer data.
@@ -1164,43 +861,30 @@ ICM20948::measure()
 
 		/*
 		 * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation
+		 * Swap axes and negate y
 		 */
-		if (_whoami != ICM_WHOAMI_20948) {
-			/*
-			 * Swap axes and negate y
-			 */
 
-			int16_t accel_xt = report.accel_y;
+		int16_t accel_xt = report.accel_y;
-			int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
+		int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
 
-			int16_t gyro_xt = report.gyro_y;
+		int16_t gyro_xt = report.gyro_y;
-			int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
+		int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
-
-			/*
-			 * Apply the swap
-			 */
-			report.accel_x = accel_xt;
-			report.accel_y = accel_yt;
-			report.gyro_x = gyro_xt;
-			report.gyro_y = gyro_yt;
-		}
 
 		/*
-		 * Report buffers.
+		 * Apply the swap
 		 */
-		sensor_accel_s		arb;
+		report.accel_x = accel_xt;
-		sensor_gyro_s			grb;
+		report.accel_y = accel_yt;
-
+		report.gyro_x = gyro_xt;
-		/*
+		report.gyro_y = gyro_yt;
-		 * Adjust and scale results to m/s^2.
-		 */
-		grb.timestamp = arb.timestamp = hrt_absolute_time();
 
 		// report the error count as the sum of the number of bad
 		// transfers and bad register reads. This allows the higher
 		// level code to decide if it should use this sensor based on
 		// whether it has had failures
-		grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
+		const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
+		_px4_accel.set_error_count(error_count);
+		_px4_gyro.set_error_count(error_count);
 
 		/*
 		 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -1218,96 +902,8 @@ ICM20948::measure()
 		 */
 
 		/* NOTE: Axes have been swapped to match the board a few lines above. */
-
+		_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
-		arb.x_raw = report.accel_x;
+		_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
-		arb.y_raw = report.accel_y;
-		arb.z_raw = report.accel_z;
-
-		float xraw_f = report.accel_x;
-		float yraw_f = report.accel_y;
-		float zraw_f = report.accel_z;
-
-		// apply user specified rotation
-		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
-
-		float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-		float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-		float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-
-		arb.x = _accel_filter_x.apply(x_in_new);
-		arb.y = _accel_filter_y.apply(y_in_new);
-		arb.z = _accel_filter_z.apply(z_in_new);
-
-		matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
-		matrix::Vector3f aval_integrated;
-
-		bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
-		arb.x_integral = aval_integrated(0);
-		arb.y_integral = aval_integrated(1);
-		arb.z_integral = aval_integrated(2);
-
-		arb.scaling = _accel_range_scale;
-
-		arb.temperature = _last_temperature;
-
-		/* return device ID */
-		arb.device_id = _accel->_device_id.devid;
-
-		grb.x_raw = report.gyro_x;
-		grb.y_raw = report.gyro_y;
-		grb.z_raw = report.gyro_z;
-
-		xraw_f = report.gyro_x;
-		yraw_f = report.gyro_y;
-		zraw_f = report.gyro_z;
-
-		// apply user specified rotation
-		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
-
-		float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-		float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-		float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
-
-		grb.x = _gyro_filter_x.apply(x_gyro_in_new);
-		grb.y = _gyro_filter_y.apply(y_gyro_in_new);
-		grb.z = _gyro_filter_z.apply(z_gyro_in_new);
-
-		matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
-		matrix::Vector3f gval_integrated;
-
-		bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
-		grb.x_integral = gval_integrated(0);
-		grb.y_integral = gval_integrated(1);
-		grb.z_integral = gval_integrated(2);
-
-		grb.scaling = _gyro_range_scale;
-
-		grb.temperature = _last_temperature;
-
-		/* return device ID */
-		grb.device_id = _gyro->_device_id.devid;
-
-		_accel_reports->force(&arb);
-		_gyro_reports->force(&grb);
-
-		/* notify anyone waiting for data */
-		if (accel_notify) {
-			_accel->poll_notify(POLLIN);
-		}
-
-		if (gyro_notify) {
-			_gyro->parent_poll_notify();
-		}
-
-		if (accel_notify && !(_accel->_pub_blocked)) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
-		}
-
-		if (gyro_notify && !(_gyro->_pub_blocked)) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
-		}
 	}
 
 	/* stop measuring */
@@ -1317,7 +913,6 @@ ICM20948::measure()
 void
 ICM20948::print_info()
 {
-	::printf("Device type:%d\n", _whoami);
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_accel_reads);
 	perf_print_counter(_gyro_reads);
@@ -1326,11 +921,11 @@ ICM20948::print_info()
 	perf_print_counter(_good_transfers);
 	perf_print_counter(_reset_retries);
 	perf_print_counter(_duplicates);
-	::printf("temperature: %.1f\n", (double)_last_temperature);
 
 	if (!_magnetometer_only) {
-		_accel_reports->print_info("accel queue");
+		_px4_accel.print_status();
-		_gyro_reports->print_info("gyro queue");
+		_px4_gyro.print_status();
-		_mag->_mag_reports->print_info("mag queue");
 	}
+
+	_mag.print_status();
 }

src/drivers/imu/icm20948/icm20948.h

@@ -31,27 +31,24 @@
  *
  ****************************************************************************/
 
-#include <board_config.h>
+#pragma once
-#include <drivers/drv_hrt.h>
+
-#include <drivers/device/ringbuffer.h>
+#include <stdint.h>
-#include <drivers/device/integrator.h>
+
-#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+#include <perf/perf_counter.h>
-#include <lib/conversion/rotation.h>
-#include <lib/ecl/geo/geo.h>
-#include <lib/perf/perf_counter.h>
-#include <px4_config.h>
-#include <px4_getopt.h>
-#include <px4_work_queue/ScheduledWorkItem.hpp>
-#include <sys/types.h>
 #include <systemlib/conversions.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/conversion/rotation.h>
 #include <systemlib/err.h>
-#include <systemlib/px4_macros.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
+
 #include <uORB/uORB.h>
 
-#include "accel.h"
+#include "ICM20948_mag.h"
-#include "gyro.h"
-#include "mag.h"
-
 
 #if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION)
 #  define USE_I2C
@@ -366,21 +363,17 @@ extern int MPU9250_probe(device::Device *dev);
 typedef device::Device *(*ICM20948_constructor)(int, uint32_t, bool);
 
 class ICM20948_mag;
-class ICM20948_accel;
-class ICM20948_gyro;
 
 class ICM20948 : public px4::ScheduledWorkItem
 {
 public:
-	ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro,
+	ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation,
-		 const char *path_mag,
-		 enum Rotation rotation,
 		 bool magnetometer_only);
 
 	virtual ~ICM20948();
 
 	virtual int		init();
-	uint8_t			get_whoami();
+	uint8_t			get_whoami() { return _whoami; }
 
 	/**
 	 * Diagnostics - print some basic information about the driver.
@@ -393,40 +386,28 @@ protected:
 
 	virtual int		probe();
 
-	friend class ICM20948_accel;
 	friend class ICM20948_mag;
-	friend class ICM20948_gyro;
 
 	void Run() override;
 
 private:
-	ICM20948_accel   *_accel;
+
-	ICM20948_gyro	*_gyro;
+	PX4Accelerometer	_px4_accel;
-	ICM20948_mag     *_mag;
+	PX4Gyroscope		_px4_gyro;
+
+	ICM20948_mag		_mag;
 	uint8_t 		_selected_bank;			/* Remember selected memory bank to avoid polling / setting on each read/write */
 	bool
 	_magnetometer_only;     /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */
 
-	unsigned		_call_interval;
+	unsigned		_call_interval{1000};
-
-	ringbuffer::RingBuffer	*_accel_reports;
-
-	struct accel_calibration_s	_accel_scale;
-	float			_accel_range_scale;
-	float			_accel_range_m_s2;
-	orb_advert_t		_accel_topic;
-
-	ringbuffer::RingBuffer	*_gyro_reports;
-
-	struct gyro_calibration_s	_gyro_scale;
-	float			_gyro_range_scale;
-	float			_gyro_range_rad_s;
 
 	unsigned		_dlpf_freq;
 	unsigned		_dlpf_freq_icm_gyro;
 	unsigned		_dlpf_freq_icm_accel;
 
-	unsigned		_sample_rate;
+	unsigned		_sample_rate{1000};
+
 	perf_counter_t		_accel_reads;
 	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
@@ -436,48 +417,32 @@ 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
 
-#ifndef MAX
-#define MAX(X,Y) ((X) > (Y) ? (X) : (Y))
-#endif
-
 	static constexpr int ICM20948_NUM_CHECKED_REGISTERS{15};
 	static const uint16_t	_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS];
 
-	const uint16_t			*_checked_registers;
+	const uint16_t			*_checked_registers{nullptr};
 
 	uint8_t					_checked_values[ICM20948_NUM_CHECKED_REGISTERS];
 	uint8_t					_checked_bad[ICM20948_NUM_CHECKED_REGISTERS];
-	unsigned				_checked_next;
+	unsigned				_checked_next{0};
-	unsigned				_num_checked_registers;
+	unsigned				_num_checked_registers{0};
 
 
 	// last temperature reading for print_info()
-	float			_last_temperature;
+	float			_last_temperature{0.0f};
 
 	bool check_null_data(uint16_t *data, uint8_t size);
 	bool check_duplicate(uint8_t *accel_data);
 	// keep last accel reading for duplicate detection
-	uint8_t			_last_accel_data[6];
+	uint8_t			_last_accel_data[6] {};
-	bool			_got_duplicate;
+	bool			_got_duplicate{false};
 
 	/**
 	 * Start automatic measurement.
@@ -619,7 +584,4 @@ private:
 	 */
 	void check_registers(void);
 
-	/* do not allow to copy this class due to pointer data members */
-	ICM20948(const ICM20948 &);
-	ICM20948 operator=(const ICM20948 &);
 };

src/drivers/imu/icm20948/icm20948_i2c.cpp

@@ -37,7 +37,9 @@
  * I2C interface for ICM20948
  */
 
+#include <px4_config.h>
 #include <drivers/device/i2c.h>
+#include <drivers/drv_device.h>
 
 #include "icm20948.h"
 

src/drivers/imu/icm20948/icm20948_main.cpp

@@ -39,11 +39,18 @@
  * based on the mpu9250 driver
  */
 
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/systemlib/conversions.h>
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <lib/conversion/rotation.h>
+
 #include "icm20948.h"
 
-#define ICM_DEVICE_PATH_ACCEL_EXT  "/dev/icm20948_accel_ext"
+#define ICM_DEVICE_PATH_EXT  "/dev/icm20948_ext"
-#define ICM_DEVICE_PATH_GYRO_EXT   "/dev/icm20948_gyro_ext"
-#define ICM_DEVICE_PATH_MAG_EXT    "/dev/icm20948_mag_ext"
 
 /** driver 'main' command */
 extern "C" { __EXPORT int icm20948_main(int argc, char *argv[]); }
@@ -69,19 +76,17 @@ namespace icm20948
 
 struct icm20948_bus_option {
 	enum ICM20948_BUS busid;
-	const char *accelpath;
+	const char *path;
-	const char *gyropath;
-	const char *magpath;
 	ICM20948_constructor interface_constructor;
 	bool magpassthrough;
 	uint8_t busnum;
 	uint32_t address;
-	ICM20948	*dev;
+	ICM20948 *dev;
 } bus_options[] = {
 #if defined (USE_I2C)
 
 #  if defined(PX4_I2C_BUS_EXPANSION)
-	{ ICM20948_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_ACCEL_EXT, ICM_DEVICE_PATH_GYRO_EXT, ICM_DEVICE_PATH_MAG_EXT, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr },
+	{ ICM20948_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_EXT, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr },
 #endif
 
 #endif
@@ -95,7 +100,6 @@ void	start(enum ICM20948_BUS busid, enum Rotation rotation, bool external_bus, b
 bool	start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only);
 struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid);
 void	stop(enum ICM20948_BUS busid);
-void	reset(enum ICM20948_BUS busid);
 void	info(enum ICM20948_BUS busid);
 void	usage();
 
@@ -120,8 +124,6 @@ struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid)
 bool
 start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external, bool magnetometer_only)
 {
-	int fd = -1;
-
 	PX4_INFO("Bus probed: %d", bus.busid);
 
 	if (bus.dev != nullptr) {
@@ -154,8 +156,7 @@ start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external
 
 #endif
 
-	bus.dev = new ICM20948(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation,
+	bus.dev = new ICM20948(interface, mag_interface, bus.path, rotation, magnetometer_only);
-			       magnetometer_only);
 
 	if (bus.dev == nullptr) {
 		delete interface;
@@ -171,33 +172,10 @@ start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external
 		goto fail;
 	}
 
-	/*
-	 * Set the poll rate to default, starts automatic data collection.
-	 * Doing this through the mag device for the time being - it's always there, even in magnetometer only mode.
-	 * Using accel device for MPU6500.
-	 */
-	fd = open(bus.magpath, O_RDONLY);
-
-	if (fd < 0) {
-		PX4_INFO("ioctl failed");
-		goto fail;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		goto fail;
-	}
-
-
-	close(fd);
-
 	return true;
 
 fail:
 
-	if (fd >= 0) {
-		close(fd);
-	}
-
 	if (bus.dev != nullptr) {
 		delete (bus.dev);
 		bus.dev = nullptr;
@@ -256,32 +234,6 @@ stop(enum ICM20948_BUS busid)
 	exit(0);
 }
 
-/**
- * Reset the driver.
- */
-void
-reset(enum ICM20948_BUS busid)
-{
-	struct icm20948_bus_option &bus = find_bus(busid);
-	int fd = open(bus.accelpath, 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.
  */
@@ -304,7 +256,7 @@ info(enum ICM20948_BUS busid)
 void
 usage()
 {
-	PX4_INFO("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'");
+	PX4_INFO("missing command: try 'start', 'info', 'stop',\n 'regdump', 'testerror'");
 	PX4_INFO("options:");
 	PX4_INFO("    -X    (i2c external bus)");
 	PX4_INFO("    -I    (i2c internal bus)");
@@ -383,13 +335,6 @@ icm20948_main(int argc, char *argv[])
 		icm20948::stop(busid);
 	}
 
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(verb, "reset")) {
-		icm20948::reset(busid);
-	}
-
 	/*
 	 * Print driver information.
 	 */

src/drivers/imu/icm20948/mag_i2c.cpp

@@ -38,7 +38,9 @@
  */
 
 #include "icm20948.h"
-#include "mag.h"
+#include "ICM20948_mag.h"
+
+#include <drivers/device/i2c.h>
 
 #ifdef USE_I2C
 
@@ -73,7 +75,7 @@ AK8963_I2C::AK8963_I2C(int bus) :
 int
 AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count)
 {
-	uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
+	uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE] {};
 
 	if (sizeof(cmd) < (count + 1)) {
 		return -EIO;
@@ -108,4 +110,4 @@ AK8963_I2C::probe()
 	return OK;
 }
 
-#endif
+#endif // USE_I2C