|
@@ -196,10 +196,7 @@ static signed int pid_calc(struct _pid *pid, int32_t busy)
|
|
|
pid->last_err = fp_error;
|
|
pid->last_err = fp_error;
|
|
|
|
|
|
|
|
result = pterm + mul_fp(pid->integral, pid->i_gain) + dterm;
|
|
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);
|
|
return (signed int)fp_toint(result);
|
|
|
}
|
|
}
|
|
|
|
|
|