Well, at least, above code is more manageable, and only two areas need to be fixed - MAKEWORD and floating point math. I'll fix that code tomorrow, compile it and give a try on a real hardware