AP_InertialSensor: Added INS_MPU6K_FILTER option

this allows the user to select the MPU6000 filtering frequency

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -12,10 +12,40 @@
 
 // Class level parameters
 const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
+    // @Param: PRODUCT_ID
+    // @DisplayName: IMU Product ID
+    // @Description: Which type of IMU is installed (read-only)
+    // @User: Standard
     AP_GROUPINFO("PRODUCT_ID",  0, AP_InertialSensor, _product_id,   0),
+
+    // @Param: ACCSCAL
+    // @DisplayName: Acceleration Scaling
+    // @Description: Calibration scaling of x/y/z acceleration axes. This is setup using the acceleration calibration
+    // @User: Advanced
     AP_GROUPINFO("ACCSCAL",     1, AP_InertialSensor, _accel_scale,  0),
+
+    // @Param: ACCOFFS
+    // @DisplayName: Acceleration Offsets
+    // @Description: Calibration offsets of x/y/z acceleration axes. This is setup using the acceleration calibration or level operations
+    // @Units: m/s/s
+    // @User: Advanced
     AP_GROUPINFO("ACCOFFS",     2, AP_InertialSensor, _accel_offset, 0),
+
+    // @Param: GYROFFS
+    // @DisplayName: Gyro offsets
+    // @Description: Calibration offsets of x/y/z gyroscope axes. This is setup on each boot during gyro calibrations
+    // @Units: rad/s
+    // @User: Advanced
     AP_GROUPINFO("GYROFFS",     3, AP_InertialSensor, _gyro_offset,  0),
+
+    // @Param: MPU6K_FILTER
+    // @DisplayName: MPU6000 filter frequency
+    // @Description: Filter frequency to ask the MPU6000 to apply to samples. This can be set to a lower value to try to cope with very high vibration levels in aircraft. The default value on ArduPlane and APMrover2 is 20Hz. The default value on ArduCopter is 42Hz. This option takes effect on the next reboot or gyro initialisation
+    // @Units: Hz
+    // @Values: 0:Default,5:5Hz,10:10Hz,20:20Hz,42:42Hz,98:98Hz
+    // @User: Advanced
+    AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter,  0),
+
     AP_GROUPEND
 };
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -175,6 +175,9 @@ protected:
     AP_Vector3f             _accel_scale;
     AP_Vector3f             _accel_offset;
     AP_Vector3f             _gyro_offset;
+
+    // filtering frequency (0 means default)
+    AP_Int8                 _mpu6000_filter;
 };
 
 #include "AP_InertialSensor_Oilpan.h"

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -402,7 +402,7 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
     register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
     delay(1);
 
-    uint8_t rate, filter;
+    uint8_t rate, filter, default_filter;
 
     // sample rate and filtering
     // to minimise the effects of aliasing we choose a filter
@@ -410,17 +410,41 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
     switch (sample_rate) {
     case RATE_50HZ:
         rate = MPUREG_SMPLRT_50HZ;
-        filter = BITS_DLPF_CFG_20HZ;
+        default_filter = BITS_DLPF_CFG_20HZ;
         break;
     case RATE_100HZ:
         rate = MPUREG_SMPLRT_100HZ;
-        filter = BITS_DLPF_CFG_42HZ;
+        default_filter = BITS_DLPF_CFG_42HZ;
         break;
     case RATE_200HZ:
     default:
         rate = MPUREG_SMPLRT_200HZ;
+        default_filter = BITS_DLPF_CFG_98HZ;
+        break;
+    }
+    
+    // choose filtering frequency
+    switch (_mpu6000_filter) {
+    case 5:
+        filter = BITS_DLPF_CFG_5HZ;
+        break;
+    case 10:
+        filter = BITS_DLPF_CFG_10HZ;
+        break;
+    case 20:
+        filter = BITS_DLPF_CFG_20HZ;
+        break;
+    case 42:
+        filter = BITS_DLPF_CFG_42HZ;
+        break;
+    case 98:
         filter = BITS_DLPF_CFG_98HZ;
         break;
+    case 0:
+    default:
+        // the user hasn't specified a specific frequency,
+        // use the default value for the given sample rate
+        filter = default_filter;
     }
 
     // set sample rate

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -60,7 +60,7 @@ private:
     static void                 data_interrupt(void);
     static uint8_t              register_read( uint8_t reg );
     static void                 register_write( uint8_t reg, uint8_t val );
-    static void                 hardware_init(Sample_rate sample_rate);
+    void                        hardware_init(Sample_rate sample_rate);
 
     float                       _temp;