/branches/V0.68d Code Redesign killagreg/fc.c |
---|
1011,6 → 1011,8 |
tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
tmp_int += (ParamSet.Yaw_P * StickYaw) / 4; |
SetPointYaw = tmp_int; |
// trimm drift of Reading_IntegralGyroYaw with SetPointYaw(StickYaw) |
Reading_IntegralGyroYaw -= tmp_int; |
// limit the effect |
if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000; |
if(Reading_IntegralGyroYaw <-50000) Reading_IntegralGyroYaw =-50000; |