From 470fcc2077bc230491660e1554df5217a4cdae5e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 16 Oct 2014 16:18:06 +0900
Subject: [PATCH] Copter: add DCM_CHECK_THRESH parameter

---
 ArduCopter/Parameters.h   | 5 ++++-
 ArduCopter/Parameters.pde | 9 ++++++++-
 ArduCopter/config.h       | 5 ++++-
 3 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index b1fe5e4df7..27eb58783e 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -120,7 +120,9 @@ public:
         k_param_ekfcheck_thresh,
         k_param_terrain,
         k_param_acro_expo,
-        k_param_throttle_deadzone,      // 57
+        k_param_throttle_deadzone,
+        k_param_optflow,
+        k_param_dcmcheck_thresh,        // 59
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -392,6 +394,7 @@ public:
 
     AP_Int8         land_repositioning;
     AP_Float        ekfcheck_thresh;
+    AP_Float        dcmcheck_thresh;
 
 #if FRAME_CONFIG ==     HELI_FRAME
     // Heli
diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde
index f894f0a0cd..9256c94010 100644
--- a/ArduCopter/Parameters.pde
+++ b/ArduCopter/Parameters.pde
@@ -456,12 +456,19 @@ const AP_Param::Info var_info[] PROGMEM = {
     GSCALAR(land_repositioning, "LAND_REPOSITION",     LAND_REPOSITION_DEFAULT),
 
     // @Param: EKF_CHECK_THRESH
-    // @DisplayName: EKF and InertialNav check compass and velocity variance threshold
+    // @DisplayName: EKF check compass and velocity variance threshold
     // @Description: Allows setting the maximum acceptable compass and velocity variance (0 to disable check)
     // @Values: 0:Disabled, 0.6:Default, 1.0:Relaxed
     // @User: Advanced
     GSCALAR(ekfcheck_thresh, "EKF_CHECK_THRESH",    EKFCHECK_THRESHOLD_DEFAULT),
 
+    // @Param: DCM_CHECK_THRESH
+    // @DisplayName: DCM yaw error threshold
+    // @Description: Allows setting the maximum acceptable yaw error as a sin of the yaw error (0 to disable check)
+    // @Values: 0:Disabled, 0.8:Default, 0.98:Relaxed
+    // @User: Advanced
+    GSCALAR(dcmcheck_thresh, "DCM_CHECK_THRESH",    DCMCHECK_THRESHOLD_DEFAULT),
+
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: HS1_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 2771c4c60e..41efe38ed1 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -327,10 +327,13 @@
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
-//  EKF Checker
+//  EKF & DCM Checker
 #ifndef EKFCHECK_THRESHOLD_DEFAULT
  # define EKFCHECK_THRESHOLD_DEFAULT    0.8f    // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad
 #endif
+#ifndef DCMCHECK_THRESHOLD_DEFAULT
+ # define DCMCHECK_THRESHOLD_DEFAULT    0.8f    // DCM checker's default yaw error threshold above which we will abandon horizontal position hold.  The units are sin(angle) so 0.8 = about 60degrees of error
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 //  MAGNETOMETER