Для ракетных задач. Цифровой ПИД нафиг не нужен. Делал я ещё в школе такое чудо. пьезо акселерометр 8 оу и на рулевые машинки. А тяга регулировалась перепуском. Всё хорошо но вот ОУ при изменении температуры один дрейфовал сильно. В общем было очень зрелищно

. Красивый старт. на высоте около 50м разворот на 90 и включение двигателя на максимальный режим. через 3 км разнесли витрину магазина
Основная проблема при проектировании это настройка ПИД. Чуть изменились начальные условия там загрузка ветер давление. И получаем перерегулирование. Имеем сильные затухающие колебания в контуре регулирования. ИМХО в простом варианте вариант работающий в полевых условиях без адаптивного алгоритма сделать сложно. А это те-же 100МИПС и 32 разряда.
Бегло прочитал датасшит на L3G4200D
В целом не плохо. Но блин I2C это блин медленно. + Много фарша но встроенные фильтры ИМХО бяка. Я бы сделал лучше на аналоге. Скорость бы получил лучше и точность.
У топикстартера проблема с i2c?
Читать и перечитывать всем заинтересованным теорию инерционных навигационных систем.
Скорость изменения скорости есть ускорение. (Блин как тут формулы вставлять)
Скорость на время есть расстояние. Если скорость угловая то расстояние есть текущий угол.
Можно и через интеграл.
Основная проблема это получение стартовой точки. 0 на координатной оси. Профи ракетчики решают просто. Ставят концевик на корпус и получается датчик схода. Алгоритм простой как 5 копеек. Рули в 0 тяга максимальная. отрываемся от стола и включаем управление рулями и двигателем. Соответственно момент отрыва от стола есть ноль координат и времени.
ИМХО Смысла адаптировать такой исходник не вижу. Там всё очень просто.