dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mpu6000: support setting the DLPF filter frequency 

APM uses this for different aircraft types
This commit is contained in:
Andrew Tridgell 2013-02-07 11:19:52 +11:00 committed by Lorenz Meier
parent 508d6d2b4f
commit 1fed72caf8
1 changed files with 41 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
apps/drivers/mpu6000/mpu6000.cpp
View File

@ -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 */