|
|
|
@ -587,7 +587,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
@@ -587,7 +587,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
|
|
|
|
|
|
|
|
|
|
// sanity check _kp_yaw
|
|
|
|
|
if (_kp_yaw < AP_AHRS_YAW_P_MIN) { |
|
|
|
|
_kp_yaw = AP_AHRS_YAW_P_MIN; |
|
|
|
|
_kp_yaw.set(AP_AHRS_YAW_P_MIN); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// update the proportional control to drag the
|
|
|
|
@ -927,7 +927,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
@@ -927,7 +927,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
|
|
|
|
|
|
|
|
|
|
// sanity check _kp value
|
|
|
|
|
if (_kp < AP_AHRS_RP_P_MIN) { |
|
|
|
|
_kp = AP_AHRS_RP_P_MIN; |
|
|
|
|
_kp.set(AP_AHRS_RP_P_MIN); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// we now want to calculate _omega_P and _omega_I. The
|
|
|
|
|