From 3b72e31e83d1075096a7ee99896885998bd3d6fd Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 09:51:37 -0700 Subject: [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 --- src/drivers/l3gd20/l3gd20.cpp | 60 +++++++++++++++++++++++++++++++++++-------- 1 file changed, 50 insertions(+), 10 deletions(-) (limited to 'src/drivers') 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 @@ -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; -- cgit v1.2.3