pid->last_err = fp_error;
result = pterm + mul_fp(pid->integral, pid->i_gain) + dterm;
- if (result >= 0)
- result = result + (1 << (FRAC_BITS-1));
- else
- result = result - (1 << (FRAC_BITS-1));
+ result = result + (1 << (FRAC_BITS-1));
return (signed int)fp_toint(result);
}