mpu6000: support setting the DLPF filter frequency

APM uses this for different aircraft types

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 */
+		_set_dlpf_filter((uint16_t)arg);
-		return -EINVAL;
+		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 */
+		_set_dlpf_filter((uint16_t)arg);
-		return -EINVAL;
+		return OK;
 
 	case GYROIOCSSCALE:
 		/* copy scale in */