From b7565affcd052539a1a3564d1ed60ee96a2865a0 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 17 Dec 2013 12:24:44 +0900
Subject: [PATCH] INS: add support for 400hz for PX4

---
 libraries/AP_InertialSensor/AP_InertialSensor.h       | 3 ++-
 libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp | 6 +++++-
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index d19738fce6..4530eb1e96 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/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.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
index 49be71c79a..2917d1342f 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
+++ b/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);