INS: add support for 400hz for PX4

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -42,7 +42,8 @@ public:
     enum Sample_rate {
         RATE_50HZ,
         RATE_100HZ,
-        RATE_200HZ
+        RATE_200HZ,
+        RATE_400HZ
     };
 
     /// Perform startup initialisation.

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -44,10 +44,14 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
         _sample_time_usec = 10000;
         break;
     case RATE_200HZ:
-    default:
         _default_filter_hz = 30;
         _sample_time_usec = 5000;
         break;
+    case RATE_400HZ:
+    default:
+        _default_filter_hz = 30;
+        _sample_time_usec = 2500;
+        break;
     }
 
     _set_filter_frequency(_mpu6000_filter);