this question might be better served over at the diy-pid-control google group. this forum is more about the ospid specifically.
but you posted here, so let’s see what we can do:
looking at your code I don’t really see a separation of the proportional and integral terms i see “error = (P_CTRL)*error;” but then this entire term is summed to old_frequency. instead of output = P*error + I *Sum(error), it looks like you have output = sum(I*P*error).
I would try to sort this out before going any further. about a year ago I posted a series of 7-8 posts detailing how I write a bullet-proof pid algorithm. it’s not going to be exactly what you need, since you’re using integer math and don’t have derivative, but maybe it can give you some ideas.
hope this helps,
Brett