Если есть какие-то наработки по коду, может поделитесь?
Вряд ли вам это поможет, т.к. автопилот не на Ардуине и код не на Си. Здесь исходники без описания (которого пока нет) https://github.com/A...opilot_MMBasic/
В общих чертах:
1. Самое главное в любом коде - это алгоритм работы, формулы.
2. Посмотрите как работает ПИД-регулятор, ну или хотя бы простой пропорциональный регулятор (на нем уже можно ездить). Игра идёт на максимально-возможное уменьшение ошибки.(ошибки тоже могут быть разные, например ошибка отклонения от курса (градусы) и ошибка отклонения от геодезической дуги заданного трека (метры) )
3. Мой автопилот возвращается обратно на трек двигаясь перпендикулярно (в первом приближении) к нему.
4. Точное перемещение по треку невозможно. Всегда будут отклонения или медленные колебания, но их можно уменьшить до нескольких метров (или меньше когда волны и ветра нет), настроив коэффициенты ПИД-регулятора (но и этого на самом деле мало, например при развороте на очередной точке на 150...180 градусов, желательно в нужный момент отключить а потом включить интегратор, предварительно его обнулив, иначе будет большая по диаметру петля разворота и потом затухающие колебания при выходе на трек).
5. Не знаю как делается правильно в промышленных автопилотах, но у меня при следовании по треку алгоритм ослика и морковки. Морковка от ослика всегда на расстоянии 60 метров. Т.е. в главном цикле каждый 1 раз в секунду вычисляется по формулам Боуринга точка трека на расстоянии 60 метров впереди лодки и азимут на неё. Одновременно для компенсации сноса от бокового ветра (течения) вычисляется кросс трак еррор XTE расстояние отклонения от трека в метрах (со знаком). XTE умножается на коэффициент 0.005 (подобран под мои условия) и входит в интегральную составляющую регулятора.