From 2620a57700b2e5f0d4747f769e3457ce14131942 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Wed, 29 Jul 2015 12:34:48 -0700
Subject: [PATCH] AP_InertialSensor: Added is_still() check

very strict check that all axis are not vibrating much at all
new param: INS_STILL_THRESH used to be a vibration threshold for different platforms
// @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 25 +++++++++++++++++++
 .../AP_InertialSensor/AP_InertialSensor.h     |  6 +++++
 2 files changed, 31 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 7bc3382e9c..61e9aaa26e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -27,12 +27,15 @@ extern const AP_HAL::HAL& hal;
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
 #define DEFAULT_GYRO_FILTER  20
 #define DEFAULT_ACCEL_FILTER 20
+#define DEFAULT_STILL_THRESH 2.5f
 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
 #define DEFAULT_GYRO_FILTER  10
 #define DEFAULT_ACCEL_FILTER 10
+#define DEFAULT_STILL_THRESH 0.1f
 #else
 #define DEFAULT_GYRO_FILTER  20
 #define DEFAULT_ACCEL_FILTER 20
+#define DEFAULT_STILL_THRESH 0.1f
 #endif
 
 #define SAMPLE_UNIT 1
@@ -289,6 +292,15 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
     AP_GROUPINFO("USE3", 22, AP_InertialSensor, _use[2],  0),
 #endif
 
+#if INS_VIBRATION_CHECK
+    // @Param: STILL_THRESH
+    // @DisplayName: Stillness threshold for detecting if we are moving
+    // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
+    // @Range: 0.05 to 50
+    // @User: Advanced
+    AP_GROUPINFO("STILL_THRESH", 23, AP_InertialSensor, _still_threshold,  DEFAULT_STILL_THRESH),
+#endif
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
@@ -1499,3 +1511,16 @@ Vector3f AP_InertialSensor::get_vibration_levels(uint8_t instance) const
     return vibe;
 }
 #endif
+
+// check for vibration movement. Return true if all axis show nearly zero movement
+bool AP_InertialSensor::is_still()
+{
+#if INS_VIBRATION_CHECK
+    Vector3f vibe = get_vibration_levels();
+    return (vibe.x < _still_threshold) &&
+           (vibe.y < _still_threshold) &&
+           (vibe.z < _still_threshold);
+#else
+    return false;
+#endif
+}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index f5f3535625..6b80d1526d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -224,6 +224,9 @@ public:
     uint32_t get_accel_clip_count(uint8_t instance) const;
 #endif
 
+    // check for vibration movement. True when all axis show nearly zero movement
+    bool is_still();
+
     /*
       HIL set functions. The minimum for HIL is set_accel() and
       set_gyro(). The others are option for higher fidelity log
@@ -355,6 +358,9 @@ private:
     uint32_t _accel_clip_count[INS_MAX_INSTANCES];
     LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
     LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];
+
+    // threshold for detecting stillness
+    AP_Float _still_threshold;
 #endif
 
     /*