Код, изменения на строках 6, 8
fix16_t divergence = knob_normalized - speed;
// БЫЛО ТАК
fix16_t proportional = fix16_mul(cfg_pid_p, divergence);
// СДЕЛАЛ ВОТ ТАК
fix16_t proportional = fix16_mul(cfg_pid_p, divergence) + knob_normalized;
if (pid_i_enabled)
{
// pid_speed_integral += (1.0 / cfg_pid_i) * divergence;
fix16_t tmp = pid_speed_integral + fix16_mul(cfg_pid_i_inv, divergence);
pid_speed_integral = fix16_clamp(
tmp,
cfg_rpm_min_limit_norm,
cfg_rpm_max_limit_norm
);
return fix16_clamp(
proportional + pid_speed_integral,
cfg_rpm_min_limit_norm,
cfg_rpm_max_limit_norm
);
}
Какой логикой я руководствовался. Нам нужно - когда divergence == 0, на выходе регулятора должно быть напряжение, соответствующее заданной скорости на холостом ходу. Поэтому я приплюсовал knob_normalized. Теперь - если divergence ноль, но с выхода регулятора идет knob_normalized, дальше по алгоритму это значение подставляется в таблицу коррекции, и вычисляется напряжение, какое надо подать чтобы была заданная скорость на холостом ходу. Если добавляем нагрузку - начинает расти divergence, растет напряжение.
Для ПИ-регулятора - это работает точно так же, интегратор будет изменять свое значение пока divergence не равна нулю, в итоге будет поддерживаться заданная скорость. Но если раньше, без коррекции, интегратор компенсировал "кривизну" работы пропорциональной части, то теперь на холостом ходу интегратор будет выходить на значение - ноль, и будет заниматься только компенсацией нагрузки.