[l3gd20] Add support for L3G4200D chip

The L3G4200D chip is very similar to the L3GD20[H] parts and can use
the same driver with minor adjustments.  There are four differences:
 * WHO_AM_I register is 0xD3 (not 0xD4 or 0xD7):
   - added an extra case to the driver probe
 * Sampling rates are marginally different:
   - setting sampling rate now depends on the detected chip
 * I2C address range is different:
   - no changes as the driver doesn't support i2c access
 * the L3G4200D has a self-test function:
   - no changes---chose not to implement feature in driver

Signed-off-by: Ash Charles <ashcharles@gmail.com>

1 files changed, 50 insertions, 10 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 90c3db9ae..a4d4c6c83 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -34,6 +34,9 @@
 /**
  * @file l3gd20.cpp
  * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ *
+ * Note: With the exception of the self-test feature, the ST L3G4200D is
+ *       also supported by this driver.
  */
 
 #include <nuttx/config.h>
@@ -89,9 +92,11 @@ static const int ERROR = -1;
 #define ADDR_WHO_AM_I			0x0F
 #define WHO_I_AM_H 				0xD7
 #define WHO_I_AM				0xD4
+#define WHO_I_AM_L3G4200D		0xD3	/* for L3G4200D */
 
 #define ADDR_CTRL_REG1			0x20
 #define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
+
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
@@ -166,6 +171,7 @@ static const int ERROR = -1;
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
 #define L3GD20_DEFAULT_RATE			760
+#define L3G4200D_DEFAULT_RATE		800
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 
@@ -216,6 +222,9 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	/* true if an L3G4200D is detected */
+	bool	_is_l3g4200d;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -331,7 +340,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
-	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
+	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_is_l3g4200d(false)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -417,8 +427,11 @@ L3GD20::probe()
 			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
 		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		/* AeroCore won't reach here but the pre-processor doesn't know this so it hits the error condition */
+		#elif CONFIG_ARCH_BOARD_AEROCORE
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
 		#else
-			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE
 		#endif
 
 		success = true;
@@ -430,6 +443,13 @@ L3GD20::probe()
 		success = true;
 	}
 
+	/* Detect the L3G4200D used on AeroCore */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+		_is_l3g4200d = true;
+		_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		success = true;
+	}
+
 	if (success)
 		return OK;
 
@@ -502,6 +522,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
+				if (_is_l3g4200d)
+					return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
 				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
@@ -683,23 +705,41 @@ L3GD20::set_samplerate(unsigned frequency)
 	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
 
 	if (frequency == 0)
-		frequency = 760;
+		if (_is_l3g4200d)
+			frequency = 800;
+		else
+			frequency = 760;
 
-	/* use limits good for H or non-H models */
+	/*
+	 * Use limits good for H or non-H models. Rates are slightly different
+	 * for L3G4200D part but register settings are the same.
+	 */
 	if (frequency <= 100) {
-		_current_rate = 95;
+		if (_is_l3g4200d)
+			_current_rate = 100;
+		else
+			_current_rate = 95;
 		bits |= RATE_95HZ_LP_25HZ;
 
 	} else if (frequency <= 200) {
-		_current_rate = 190;
+		if (_is_l3g4200d)
+			_current_rate = 200;
+		else
+			_current_rate = 190;
 		bits |= RATE_190HZ_LP_50HZ;
 
 	} else if (frequency <= 400) {
-		_current_rate = 380;
+		if (_is_l3g4200d)
+			_current_rate = 400;
+		else
+			_current_rate = 380;
 		bits |= RATE_380HZ_LP_50HZ;
 
 	} else if (frequency <= 800) {
-		_current_rate = 760;
+		if (_is_l3g4200d)
+			_current_rate = 800;
+		else
+			_current_rate = 760;
 		bits |= RATE_760HZ_LP_50HZ;
 	} else {
 		return -EINVAL;
@@ -772,7 +812,7 @@ L3GD20::reset()
 	 * callback fast enough to not miss data. */
 	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
 
-	set_samplerate(0); // 760Hz
+	set_samplerate(0); // 760Hz or 800Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
 	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
 
@@ -971,7 +1011,7 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+	g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
 
 	if (g_dev == nullptr)
 		goto fail;