From 8ceccd778d84ddfd31de21d49425009c06abb51b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 12 Jun 2015 18:23:38 +0900
Subject: [PATCH] InertialSensor: disable vibration checks on APM2

Also bug fix get_accel_clip_count's instance check
---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp | 10 +++++++++-
 libraries/AP_InertialSensor/AP_InertialSensor.h   |  6 ++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index d1cc5b407f..b23c1ef381 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -285,8 +285,10 @@ AP_InertialSensor::AP_InertialSensor() :
     _hil_mode(false),
     _calibrating(false),
     _log_raw_data(false),
+#if INS_VIBRATION_CHECK
     _accel_vibe_floor_filter(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ),
     _accel_vibe_filter(AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ),
+#endif
     _dataflash(NULL)
 {
     AP_Param::setup_object_defaults(this, var_info);        
@@ -296,7 +298,9 @@ AP_InertialSensor::AP_InertialSensor() :
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         _accel_error_count[i] = 0;
         _gyro_error_count[i] = 0;
+#if INS_VIBRATION_CHECK
         _accel_clip_count[i] = 0;
+#endif
     }
     memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
     memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
@@ -643,14 +647,16 @@ AP_InertialSensor::init_gyro()
     _save_parameters();
 }
 
+#if INS_VIBRATION_CHECK
 // accelerometer clipping reporting
 uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
 {
-    if (instance > get_accel_count()) {
+    if (instance >= get_accel_count()) {
         return 0;
     }
     return _accel_clip_count[instance];
 }
+#endif
 
 // get_gyro_health_all - return true if all gyros are healthy
 bool AP_InertialSensor::get_gyro_health_all(void) const
@@ -1330,6 +1336,7 @@ void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
     }
 }
 
+#if INS_VIBRATION_CHECK
 // calculate vibration levels and check for accelerometer clipping (called by a backends)
 void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt)
 {
@@ -1365,3 +1372,4 @@ Vector3f AP_InertialSensor::get_vibration_levels() const
     vibe.z = safe_sqrt(vibe.z);
     return vibe;
 }
+#endif
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 102bb04531..c10a785cc9 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -17,9 +17,11 @@
 #if HAL_CPU_CLASS > HAL_CPU_CLASS_16
 #define INS_MAX_INSTANCES 3
 #define INS_MAX_BACKENDS  6
+#define INS_VIBRATION_CHECK 1
 #else
 #define INS_MAX_INSTANCES 1
 #define INS_MAX_BACKENDS  1
+#define INS_VIBRATION_CHECK 0
 #endif
 
 
@@ -220,6 +222,7 @@ public:
     // enable/disable raw gyro/accel logging
     void set_raw_logging(bool enable) { _log_raw_data = enable; }
 
+#if INS_VIBRATION_CHECK
     // calculate vibration levels and check for accelerometer clipping (called by a backends)
     void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
 
@@ -228,6 +231,7 @@ public:
 
     // retrieve and clear accelerometer clipping count
     uint32_t get_accel_clip_count(uint8_t instance) const;
+#endif
 
 private:
 
@@ -336,10 +340,12 @@ private:
     uint32_t _accel_error_count[INS_MAX_INSTANCES];
     uint32_t _gyro_error_count[INS_MAX_INSTANCES];
 
+#if INS_VIBRATION_CHECK
     // vibration and clipping
     uint32_t _accel_clip_count[INS_MAX_INSTANCES];
     LowPassFilterVector3f _accel_vibe_floor_filter;
     LowPassFilterVector3f _accel_vibe_filter;
+#endif
 
     DataFlash_Class *_dataflash;
 };