AHRS_DCM: sanity check AHRS_RP_P and AHRS_YAW_P

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,

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.