1407,8 → 1407,8 |
DiffRoll = Reading_GyroRoll - StickRoll; // get difference |
if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - StickRoll; // I-part for attitude control |
else SumRoll += DiffRoll; // I-part for head holding |
if(SumRoll > 16000) SumRoll = 16000; |
if(SumRoll < -16000) SumRoll = -16000; |
if(SumRoll > (STICK_GAIN * 16000L)) SumRoll = (STICK_GAIN * 16000L); |
if(SumRoll < -(STICK_GAIN * 16000L)) SumRoll = -(STICK_GAIN * 16000L); |
pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll |
tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
if(pd_result > tmp_int) pd_result = tmp_int; |