diff options
Diffstat (limited to 'src/drivers/l3gd20/l3gd20.cpp')
-rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 126 |
1 files changed, 102 insertions, 24 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 744abfa00..de6b753f1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -59,11 +59,11 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> -#include <arch/board/board.h> - #include <drivers/device/spi.h> #include <drivers/drv_gyro.h> +#include <board_config.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -71,6 +71,12 @@ #endif static const int ERROR = -1; +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -78,6 +84,7 @@ static const int ERROR = -1; /* register addresses */ #define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM_H 0xD7 #define WHO_I_AM 0xD4 #define ADDR_CTRL_REG1 0x20 @@ -85,8 +92,8 @@ static const int ERROR = -1; /* 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) | (1<<5) | (1<<4)) -#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -185,9 +192,14 @@ private: unsigned _current_rate; unsigned _current_range; + unsigned _orientation; perf_counter_t _sample_perf; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + /** * Start automatic measurement. */ @@ -285,7 +297,11 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_topic(-1), _current_rate(0), _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) + _orientation(SENSOR_BOARD_ROTATION_270_DEG), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(250, 30), + _gyro_filter_y(250, 30), + _gyro_filter_z(250, 30) { // enable debug() calls _debug_enabled = true; @@ -340,7 +356,6 @@ L3GD20::init() write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ /* disable FIFO. This makes things simpler and ensures we @@ -348,7 +363,7 @@ L3GD20::init() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_range(500); /* default to 500dps */ + set_range(2000); /* default to 2000dps */ set_samplerate(0); /* max sample rate */ ret = OK; @@ -362,9 +377,24 @@ L3GD20::probe() /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); - /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 + _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 + #endif return OK; + } + + + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + _orientation = SENSOR_BOARD_ROTATION_180_DEG; + return OK; + } return -EIO; } @@ -453,6 +483,13 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; + // adjust filters + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + /* if we need to start the poll state machine, do it */ if (want_start) start(); @@ -505,10 +542,17 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGSAMPLERATE: return _current_rate; - case GYROIOCSLOWPASS: + case GYROIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + return OK; + } + case GYROIOCGLOWPASS: - /* XXX not implemented due to wacky interaction with samplerate */ - return -EINVAL; + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -573,28 +617,32 @@ int L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; + float new_range_scale_dps_digit; - if (max_dps == 0) + if (max_dps == 0) { max_dps = 2000; - + } if (max_dps <= 250) { _current_range = 250; bits |= RANGE_250DPS; + new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { _current_range = 500; bits |= RANGE_500DPS; + new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { _current_range = 2000; bits |= RANGE_2000DPS; + new_range_scale_dps_digit = 70e-3f; } else { return -EINVAL; } _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; - _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); return OK; @@ -608,19 +656,20 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - if (frequency <= 95) { + // use limits good for H or non-H models + if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; - } else if (frequency <= 190) { + } else if (frequency <= 200) { _current_rate = 190; bits |= RATE_190HZ_LP_25HZ; - } else if (frequency <= 380) { + } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_30HZ; + bits |= RATE_380HZ_LP_20HZ; - } else if (frequency <= 760) { + } else if (frequency <= 800) { _current_rate = 760; bits |= RATE_760HZ_LP_30HZ; @@ -701,14 +750,43 @@ L3GD20::measure() */ report->timestamp = hrt_absolute_time(); - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + switch (_orientation) { + + case SENSOR_BOARD_ROTATION_000_DEG: + /* keep axes in place */ + report->x_raw = raw_report.x; + report->y_raw = raw_report.y; + break; + + case SENSOR_BOARD_ROTATION_090_DEG: + /* swap x and y */ + report->x_raw = raw_report.y; + report->y_raw = raw_report.x; + break; + + case SENSOR_BOARD_ROTATION_180_DEG: + /* swap x and y and negate both */ + report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + break; + + case SENSOR_BOARD_ROTATION_270_DEG: + /* swap x and y and negate y */ + report->x_raw = raw_report.y; + report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + break; + } + report->z_raw = raw_report.z; report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + report->x = _gyro_filter_x.apply(report->x); + report->y = _gyro_filter_y.apply(report->y); + report->z = _gyro_filter_z.apply(report->z); + report->scaling = _gyro_range_scale; report->range_rad_s = _gyro_range_rad_s; @@ -782,7 +860,7 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + errx(0, "already started"); /* create the driver */ g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); |