ICM20948 integration into MPU9250 driver

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -100,6 +100,9 @@ then
 	teraranger start -a
 fi
 
+# ICM20948 as external magnetometer on I2C (e.g. Here GPS)
+mpu9250 -X -M -R 6 start
+
 ###############################################################################
 #                            End Optional drivers                             #
 ###############################################################################

src/drivers/imu/mpu9250/CMakeLists.txt

@@ -40,6 +40,7 @@ px4_add_module(
 		mpu9250_i2c.cpp
 		mpu9250_spi.cpp
 		main.cpp
+		accel.cpp
 		gyro.cpp
 		mag.cpp
 		mag_i2c.cpp

src/drivers/imu/mpu9250/accel.cpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   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 <px4_config.h>
+#include <ecl/geo/geo.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <stdio.h>
+
+#include <perf/perf_counter.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 "mag.h"
+#include "gyro.h"
+#include "mpu9250.h"
+
+MPU9250_accel::MPU9250_accel(MPU9250 *parent, const char *path) :
+	CDev("MPU9250_accel", path),
+	_parent(parent)
+{
+}
+
+MPU9250_accel::~MPU9250_accel()
+{
+	if (_accel_class_instance != -1) {
+		unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
+	}
+}
+
+int
+MPU9250_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
+MPU9250_accel::parent_poll_notify()
+{
+	poll_notify(POLLIN);
+}
+
+int
+MPU9250_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	/*
+	 * Repeated in MPU9250_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/mpu9250/accel.h

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   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
+
+class MPU9250;
+
+/**
+ * Helper class implementing the accel driver node.
+ */
+class MPU9250_accel : public device::CDev
+{
+public:
+	MPU9250_accel(MPU9250 *parent, const char *path);
+	~MPU9250_accel();
+
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	virtual int		init();
+
+protected:
+	friend class MPU9250;
+
+	void			parent_poll_notify();
+
+private:
+	MPU9250			*_parent;
+
+	orb_advert_t		_accel_topic{nullptr};
+	int			_accel_orb_class_instance{-1};
+	int			_accel_class_instance{-1};
+
+};

src/drivers/imu/mpu9250/gyro.cpp

@@ -42,27 +42,14 @@
  */
 
 #include <px4_config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <stdio.h>
-
-#include <perf/perf_counter.h>
-
-#include <board_config.h>
-#include <drivers/drv_hrt.h>
-
+#include <lib/perf/perf_counter.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/mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/conversion/rotation.h>
 
 #include "mag.h"
@@ -71,10 +58,7 @@
 
 MPU9250_gyro::MPU9250_gyro(MPU9250 *parent, const char *path) :
 	CDev("MPU9250_gyro", path),
-	_parent(parent),
-	_gyro_topic(nullptr),
-	_gyro_orb_class_instance(-1),
-	_gyro_class_instance(-1)
+	_parent(parent)
 {
 }
 
@@ -88,10 +72,8 @@ MPU9250_gyro::~MPU9250_gyro()
 int
 MPU9250_gyro::init()
 {
-	int ret;
-
 	// do base class init
-	ret = CDev::init();
+	int ret = CDev::init();
 
 	/* if probe/setup failed, bail now */
 	if (ret != OK) {
@@ -110,22 +92,22 @@ MPU9250_gyro::parent_poll_notify()
 	poll_notify(POLLIN);
 }
 
-ssize_t
-MPU9250_gyro::read(struct file *filp, char *buffer, size_t buflen)
-{
-	return _parent->gyro_read(filp, buffer, buflen);
-}
-
 int
 MPU9250_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-
 	switch (cmd) {
-	case DEVIOCGDEVICEID:
-		return (int)CDev::ioctl(filp, cmd, arg);
-		break;
+
+	/* 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 _parent->gyro_ioctl(filp, cmd, arg);
+		return CDev::ioctl(filp, cmd, arg);
 	}
 }

src/drivers/imu/mpu9250/gyro.h

@@ -44,7 +44,6 @@ class MPU9250_gyro : public device::CDev
 	MPU9250_gyro(MPU9250 *parent, const char *path);
 	~MPU9250_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();
@@ -56,11 +55,8 @@ class MPU9250_gyro : public device::CDev
 
 private:
 	MPU9250			*_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 */
-	MPU9250_gyro(const MPU9250_gyro &);
-	MPU9250_gyro operator=(const MPU9250_gyro &);
+	orb_advert_t		_gyro_topic{nullptr};
+	int			_gyro_orb_class_instance{-1};
+	int			_gyro_class_instance{-1};
 };