В общем была проблема с неправильным снятием (с ошибками) карты высот.
Это выражалось во внезапной остановки в плюсе по оси Z, либо плавала высота в одной и той же точке.
Доходило до того, что мог по 20 раз снимать высоту на одной точке и она каждый раз была разной.
Поспрашивав тут у местных мне посоветовали повесить оптрон на PIN A5, что я и сделал. Добавил подтяжку 10KOhm, запитался от разъема для OFF-Line контроллера.
Были опасения, что оптрон внесет погрешность в виде задержки на открытие транзистора, который замкнет A5 на землю, но как показала практика зря.
В итоге разброс при 10 кратном снятии высоты в одной и той же точке составила 0.001 мм.
Карта высот теперь снимается идеально, и мне теперь не надо часами ковыряться с ручной правкой карты.
На всякий случай файлик SprintLayout 6. Думаю схему устройства рисовать не надо.
