From 39c8535223237d179e017a9e42db8d0a5dd05a35 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 21 Oct 2014 21:40:16 +0900
Subject: [PATCH] AHRS_DCM: sanity check AHRS_RP_P and AHRS_YAW_P

---
 libraries/AP_AHRS/AP_AHRS.h       |  2 ++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 ++++++++++
 2 files changed, 12 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 597e6fe33f..c223c3920d 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -32,6 +32,8 @@
 #include <AP_Param.h>
 
 #define AP_AHRS_TRIM_LIMIT 10.0f        // maximum trim angle in degrees
+#define AP_AHRS_RP_P_MIN   0.05f        // minimum value for AHRS_RP_P parameter
+#define AP_AHRS_YAW_P_MIN  0.05f        // minimum value for AHRS_YAW_P parameter
 
 enum AHRS_VehicleClass {
     AHRS_VEHICLE_UNKNOWN,
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 6d5510e096..72743e0eb8 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -442,6 +442,11 @@ AP_AHRS_DCM::drift_correction_yaw(void)
     // integration at higher rates
     float spin_rate = _omega.length();
 
+    // sanity check _kp_yaw
+    if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
+        _kp_yaw = AP_AHRS_YAW_P_MIN;
+    }
+
     // update the proportional control to drag the
     // yaw back to the right value. We use a gain
     // that depends on the spin rate. See the fastRotations.pdf
@@ -715,6 +720,11 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // base the P gain on the spin rate
     float spin_rate = _omega.length();
 
+    // sanity check _kp value
+    if (_kp < AP_AHRS_RP_P_MIN) {
+        _kp = AP_AHRS_RP_P_MIN;
+    }
+
     // we now want to calculate _omega_P and _omega_I. The
     // _omega_P value is what drags us quickly to the
     // accelerometer reading.