Адаптация Marlin для измерения кривизны поверхности

Вопрос в продолжение этого: https://3dtoday.ru/questions/neskolko-datcikov-bltouch-podklyucit-k-arduino-kak .

Повторюсь, что задача в измерении достаточно кривой поверхности (типа лопасти гребного винта). Принцип тот же, что и измерение кривизны стола у принтера с автоуровнем. Щуп контактный, деталь электропроводная.

Хочу попробовать приспособить для этой цели 3D-принтер. У меня есть старенькая Пруша и как раз с контактным датчиком типа https://3dtoday.ru/blogs/mad3d/blintouch/ . Заказал комплект электроники управления принтером (Arduino Mega 2560 R3 + RAMPS 1.4 + DRV8825 + LCD 12864. Если я правильно понимаю, то проще всего вставить этот алгоритм (см. ниже) в прошивку Marlin, отключив ненужные функции – нагрев, с его контролем, управление экструдером и еще что-то (что, именно?) Но мой опыт работы с Marlin ограничивается настройкой файла конфигурации с последующей заливкой в плату. Хотя имею богатый опыт программирования на FORTRAN, средний на MATLAB и небольшой на C. Как же мне начать реализацию алгоритма - в какие файлы добавлять код?

Примерный алгоритм работы программы1.Запуск (из меню принтера)2. Считываем исходные данные с SD-карты:      Количество точек по X (Nx)       Количество точек по Y(Ny)       Координаты начальной точки (xn, yn)       Высота детали (с запасом)       Количество повторений измерения в каждой точке3. Вычисляем координатную сетку (x,y) 4. Открываем файл для записи на SD-карте5. Паркуем измерительный датчик в точку (x0, y0, z0) 6. Переходим в начальную точку (xn, yn)7. Поднимаем датчик на высоту детали + высота щупа. 8. Опускаем до срабатывания щупа. 9. Поднимаем на 3 мм вверх10. Опускаем медленно до срабатывания щупа11. Повторяем пункты 7-10 заданное число раз 12. Вычисляем среднее значение z13. Записываем x, y, z на SD карту. 14. После измерения в последней точке закрываем файл на SD карте и останавливаем программу.

Спасибо.