AP_InertialSensor: added filter frequency support to PX4 driver

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -29,13 +29,16 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
     switch (sample_rate) {
     case RATE_50HZ:
         _sample_divider = 4;
+        _default_filter_hz = 10;
         break;
     case RATE_100HZ:
         _sample_divider = 2;
+        _default_filter_hz = 42;
         break;
     case RATE_200HZ:
     default:
         _sample_divider = 1;
+        _default_filter_hz = 42;
         break;
     }
 
@@ -67,9 +70,23 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
     // register a 1kHz timer to read from PX4 sensor drivers
     hal.scheduler->register_timer_process(_ins_timer);
 
+    _set_filter_frequency(_mpu6000_filter);
+
     return AP_PRODUCT_ID_PX4;
 }
 
+/*
+  set the filter frequency
+ */
+void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
+{
+    if (filter_hz == 0) {
+        filter_hz = _default_filter_hz;
+    }
+    ioctl(_gyro_fd,  GYROIOCSLOWPASS,  filter_hz);
+    ioctl(_accel_fd, ACCELIOCSLOWPASS, filter_hz);
+}
+
 /*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
 
 bool AP_InertialSensor_PX4::update(void) 
@@ -106,6 +123,11 @@ bool AP_InertialSensor_PX4::update(void)
     _gyro.rotate(_board_orientation);
     _gyro -= _gyro_offset;
 
+    if (_last_filter_hz != _mpu6000_filter) {
+        _set_filter_frequency(_mpu6000_filter);
+        _last_filter_hz = _mpu6000_filter;
+    }
+
     return true;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -41,6 +41,12 @@ private:
     static uint64_t _last_gyro_timestamp;
     uint8_t  _sample_divider;
 
+    // support for updating filter at runtime
+    uint8_t _last_filter_hz;
+    uint8_t _default_filter_hz;
+
+    void _set_filter_frequency(uint8_t filter_hz);
+
     // accelerometer and gyro driver handles
     static int _accel_fd;
     static int _gyro_fd;