diff options
| author | Andrew Tridgell <tridge@samba.org> | 2013-02-07 11:19:52 +1100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-02-08 16:21:00 +0100 |
| commit | 1fed72caf8679dae6748dffa597086cdb0644218 (patch) | |
| tree | 844b436b899753c6168c489f15ae9f46815eb024 /apps/drivers/mpu6000/mpu6000.cpp | |
| parent | 508d6d2b4f86db5224b6201cb5a8bda3b6e2a2b8 (diff) | |
| download | px4-firmware-1fed72caf8679dae6748dffa597086cdb0644218.tar.gz px4-firmware-1fed72caf8679dae6748dffa597086cdb0644218.tar.bz2 px4-firmware-1fed72caf8679dae6748dffa597086cdb0644218.zip | |
mpu6000: support setting the DLPF filter frequency
APM uses this for different aircraft types
Diffstat (limited to 'apps/drivers/mpu6000/mpu6000.cpp')
| -rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 46 |
1 files changed, 41 insertions, 5 deletions
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 574300639..27e200e40 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -267,6 +267,11 @@ private: */ int self_test(); + /* + set low pass filter frequency + */ + void _set_dlpf_filter(uint16_t frequency_hz); + }; /** @@ -379,7 +384,7 @@ MPU6000::init() // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response - write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_20HZ); + _set_dlpf_filter(20); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -488,6 +493,37 @@ MPU6000::probe() return -EIO; } +/* + set the DLPF filter frequency. This affects both accel and gyro. + */ +void +MPU6000::_set_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + /* + choose next highest filter frequency available + */ + if (frequency_hz <= 5) { + filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { + filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { + filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 42) { + filter = BITS_DLPF_CFG_42HZ; + } else if (frequency_hz <= 98) { + filter = BITS_DLPF_CFG_98HZ; + } else if (frequency_hz <= 188) { + filter = BITS_DLPF_CFG_188HZ; + } else if (frequency_hz <= 256) { + filter = BITS_DLPF_CFG_256HZ_NOLPF2; + } else { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } + write_reg(MPUREG_CONFIG, filter); +} + ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { @@ -613,8 +649,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: - /* XXX not implemented */ - return -EINVAL; + _set_dlpf_filter((uint16_t)arg); + return OK; case ACCELIOCSSCALE: { @@ -671,8 +707,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: - /* XXX not implemented */ - return -EINVAL; + _set_dlpf_filter((uint16_t)arg); + return OK; case GYROIOCSSCALE: /* copy scale in */ |