опять лаконично, т.е. решили продолжить самостоятельно изучать, набивать шишки и надеяться, что кто-то случайно упоминёт подходящее решение или угадает, что сделать хотите.
хороший выбор.
а что поправлять? у вас магнитометр = датчик азимута = гоночный полётный контроллер или любой другой автопилот/полётный контроллер. "смешались в кучу кони, люди, и залпы тысячи орудий..."
как-то так.
cистема определения курса и пространственного положения это конечно круто, звучит. а подходит ли конкретная реализация для задачи, это другой момент. хотя очень оригинальная идея поставить один полётный контроллер, что бы прикрутить к нему гнсс, задать план полёта и потом через одно место выгребать данные на ещё какой-то автопилот... или в пень гнсс или ещё какую систему позиционирования и настройку маршрута? а что тогда выгребать из этого полётного контроллера собрались? магнитный меридиан,
азимут рысканье или ещё что? если не дошло про лаконичность - эта поделка нужна вам, так что старый добрый терморектальный криптоанализатор использовать не целесообразно. самостоятельное изучение и проработка это хорошо, но пока получается примерно так - то ли вывих, то ли простуда, какие капли для глаз или аппарат элизарова нужен?