drivers: re-use calibration topic

E.g. instead of defining a gyro_scale struct in drv_gyro.h, use the
gyro_calibration message.

src/drivers/bma180/bma180.cpp

@@ -149,7 +149,7 @@ private:
 
 	RingBuffer		*_reports;
 
-	struct accel_scale	_accel_scale;
+	struct accel_calibration_s	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
@@ -496,12 +496,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
+		memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCSRANGE:

src/drivers/drv_accel.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   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
@@ -42,6 +42,7 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
+#include <uORB/topics/accel_calibration.h>
 
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
@@ -54,16 +55,6 @@
 #include <uORB/topics/sensor_accel.h>
 #define accel_report sensor_accel_s
 
-/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
-struct accel_scale {
-	float	x_offset;
-	float	x_scale;
-	float	y_offset;
-	float	y_scale;
-	float	z_offset;
-	float	z_scale;
-};
-
 /*
  * ioctl() definitions
  *

src/drivers/drv_gyro.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   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
@@ -42,6 +42,7 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
+#include <uORB/topics/gyro_calibration.h>
 
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
@@ -54,15 +55,6 @@
 #include <uORB/topics/sensor_gyro.h>
 #define gyro_report sensor_gyro_s
 
-/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
-struct gyro_scale {
-	float	x_offset;
-	float	x_scale;
-	float	y_offset;
-	float	y_scale;
-	float	z_offset;
-	float	z_scale;
-};
 
 /*
  * ioctl() definitions

src/drivers/drv_mag.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   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
@@ -40,6 +40,7 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
+#include <uORB/topics/mag_calibration.h>
 
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
@@ -52,15 +53,6 @@
 #include <uORB/topics/sensor_mag.h>
 #define mag_report sensor_mag_s
 
-/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
-struct mag_scale {
-	float	x_offset;
-	float	x_scale;
-	float	y_offset;
-	float	y_scale;
-	float	z_offset;
-	float	z_scale;
-};
 
 /*
  * ioctl() definitions

src/drivers/hmc5883/hmc5883.cpp

@@ -1075,23 +1075,21 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	// XXX do something smarter here
 	int fd = (int)enable;
 
-	struct mag_scale mscale_previous = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
+	struct mag_calibration_s mscale_previous;
+	mscale_previous.x_offset = 0.0f,
+	mscale_previous.x_scale = 1.0f,
+	mscale_previous.y_offset = 0.0f,
+	mscale_previous.y_scale = 1.0f,
+	mscale_previous.z_offset = 0.0f,
+	mscale_previous.z_scale = 1.0f,
 
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
+	struct mag_calibration_s mscale_null;
+	mscale_null.x_offset = 0.0f,
+	mscale_null.x_scale = 1.0f,
+	mscale_null.y_offset = 0.0f,
+	mscale_null.y_scale = 1.0f,
+	mscale_null.z_offset = 0.0f,
+	mscale_null.z_scale = 1.0f,
 
 	float sum_excited[3] = {0.0f, 0.0f, 0.0f};
 

src/drivers/l3gd20/l3gd20.cpp

@@ -235,7 +235,7 @@ private:
 
 	ringbuffer::RingBuffer	*_reports;
 
-	struct gyro_scale	_gyro_scale;
+	struct gyro_calibration_s	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
@@ -704,12 +704,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
+		memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
+		memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCSRANGE:

src/drivers/lsm303d/lsm303d.cpp

@@ -279,13 +279,13 @@ private:
 	ringbuffer::RingBuffer	*_accel_reports;
 	ringbuffer::RingBuffer		*_mag_reports;
 
-	struct accel_scale	_accel_scale;
+	struct accel_calibration_s	_accel_scale;
 	unsigned		_accel_range_m_s2;
 	float			_accel_range_scale;
 	unsigned		_accel_samplerate;
 	unsigned		_accel_onchip_filter_bandwith;
 
-	struct mag_scale	_mag_scale;
+	struct mag_calibration_s	_mag_scale;
 	unsigned		_mag_range_ga;
 	float			_mag_range_scale;
 	unsigned		_mag_samplerate;
@@ -962,7 +962,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSCALE: {
 			/* copy scale, but only if off by a few percent */
-			struct accel_scale *s = (struct accel_scale *) arg;
+			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
 			float sum = s->x_scale + s->y_scale + s->z_scale;
 
 			if (sum > 2.0f && sum < 4.0f) {
@@ -984,7 +984,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCSELFTEST:
@@ -1095,12 +1095,12 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case MAGIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
+		memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale));
 		return OK;
 
 	case MAGIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
+		memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale));
 		return OK;
 
 	case MAGIOCSRANGE:

src/drivers/mpu6000/mpu6000.cpp

@@ -267,7 +267,7 @@ private:
 
 	ringbuffer::RingBuffer	*_accel_reports;
 
-	struct accel_scale	_accel_scale;
+	struct accel_calibration_s	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
@@ -276,7 +276,7 @@ private:
 
 	ringbuffer::RingBuffer	*_gyro_reports;
 
-	struct gyro_scale	_gyro_scale;
+	struct gyro_calibration_s	_gyro_scale;
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 
@@ -1400,7 +1400,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSCALE: {
 			/* copy scale, but only if off by a few percent */
-			struct accel_scale *s = (struct accel_scale *) arg;
+			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
 			float sum = s->x_scale + s->y_scale + s->z_scale;
 
 			if (sum > 2.0f && sum < 4.0f) {
@@ -1414,7 +1414,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCSRANGE:
@@ -1484,12 +1484,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
+		memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
+		memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCSRANGE:

src/drivers/mpu9250/mpu9250.cpp

@@ -194,6 +194,250 @@
  */
 #define MPU9250_TIMER_REDUCTION				200
 
+class MPU9250_gyro;
+
+class MPU9250 : public device::SPI
+{
+public:
+	MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
+	virtual ~MPU9250();
+
+	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.
+	 */
+	void			print_info();
+
+	void			print_registers();
+
+	// deliberately cause a sensor error
+	void 			test_error();
+
+protected:
+	virtual int		probe();
+
+	friend class MPU9250_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:
+	MPU9250_gyro		*_gyro;
+	uint8_t			_whoami;	/** whoami result */
+
+	struct hrt_call		_call;
+	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;
+
+	unsigned		_sample_rate;
+	perf_counter_t		_accel_reads;
+	perf_counter_t		_gyro_reads;
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_bad_transfers;
+	perf_counter_t		_bad_registers;
+	perf_counter_t		_good_transfers;
+	perf_counter_t		_reset_retries;
+	perf_counter_t		_duplicates;
+	perf_counter_t		_controller_latency_perf;
+
+	uint8_t			_register_wait;
+	uint64_t		_reset_wait;
+
+	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 MPU9250_NUM_CHECKED_REGISTERS 11
+	static const uint8_t	_checked_registers[MPU9250_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_values[MPU9250_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_bad[MPU9250_NUM_CHECKED_REGISTERS];
+	uint8_t			_checked_next;
+
+	// last temperature reading for print_info()
+	float			_last_temperature;
+
+	// keep last accel reading for duplicate detection
+	uint16_t		_last_accel[3];
+	bool			_got_duplicate;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	int			reset();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Fetch measurements from the sensor and update the report buffers.
+	 */
+	void			measure();
+
+	/**
+	 * Read a register from the MPU9250
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED);
+	uint16_t		read_reg16(unsigned reg);
+
+	/**
+	 * Write a register in the MPU9250
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the MPU9250
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Write a register in the MPU9250, updating _checked_values
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_checked_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Set the MPU9250 measurement range.
+	 *
+	 * @param max_g		The maximum G value the range must support.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_accel_range(unsigned max_g);
+
+	/**
+	 * Swap a 16-bit value read from the MPU9250 to native byte order.
+	 */
+	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
+
+	/**
+	 * Get the internal / external state
+	 *
+	 * @return true if the sensor is not on the main MCU board
+	 */
+	bool			is_external() { return (_bus == EXTERNAL_BUS); }
+
+	/**
+	 * Measurement self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			self_test();
+
+	/**
+	 * Accel self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			accel_self_test();
+
+	/**
+	 * Gyro self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	int 			gyro_self_test();
+
+	/*
+	  set low pass filter frequency
+	 */
+	void _set_dlpf_filter(uint16_t frequency_hz);
+
+	/*
+	  set sample rate (approximate) - 1kHz to 5Hz
+	*/
+	void _set_sample_rate(unsigned desired_sample_rate_hz);
+
+	/*
+	  check that key registers still have the right value
+	 */
+	void check_registers(void);
+
+	/* do not allow to copy this class due to pointer data members */
+	MPU9250(const MPU9250 &);
+	MPU9250 operator=(const MPU9250 &);
+
+#pragma pack(push, 1)
+	/**
+	 * Report conversation within the MPU9250, including command byte and
+	 * interrupt status.
+	 */
+	struct MPUReport {
+		uint8_t		cmd;
+		uint8_t		status;
+		uint8_t		accel_x[2];
+		uint8_t		accel_y[2];
+		uint8_t		accel_z[2];
+		uint8_t		temp[2];
+		uint8_t		gyro_x[2];
+		uint8_t		gyro_y[2];
+		uint8_t		gyro_z[2];
+	};
+#pragma pack(pop)
+};
 
 /*
   list of registers that will be checked in check_registers(). Note
@@ -910,7 +1154,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSCALE: {
 			/* copy scale, but only if off by a few percent */
-			struct accel_scale *s = (struct accel_scale *) arg;
+			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
 			float sum = s->x_scale + s->y_scale + s->z_scale;
 
 			if (sum > 2.0f && sum < 4.0f) {
@@ -924,7 +1168,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
+		memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCSRANGE:
@@ -1006,12 +1250,12 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_scale *) arg, sizeof(_gyro_scale));
+		memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct gyro_scale *) arg, &_gyro_scale, sizeof(_gyro_scale));
+		memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCSRANGE: