У нас реализовано такое на экскаваторе. Могу скинуть описание. Может натолкнет на что-нибудь. Исходник тоже могу скинуть, но они на ассемблере и сильно путанные.
E-mail: info@telesys.ru