Реализовать алгоритм Маджвика под Embedded Linux

Добрый день.
Имеется железка на базе процессора IMX6ULL под управлением ОС Embedded linux (OpenWrt). На борту следующая периферия:

• lps22hb - датчик давления
• lsm6ds3tr - 3-ех осевой акселерометр + 3-ех осевой гироскоп
• lis3mdl - 3-ех осевой магнетометр

Поддержка данных датчиков в ядре реализована. Достаточно включить их в ядре (в теории).
Необходимо разработать пользовательское приложение, реализующее алгоритм Маджвика для пространственной ориентации полетного контроллера (данной железки).
Пример реализации алгоритма на Си с теми отличиями, что он реализован под bare metal и на основе 9-ти осевого датчика (3-ех осевой акселерометр + 3-ех осевой гироскоп + 3-ех осевой магнетометр в одном корпусе с данными, запрашиваемыми единым пакетом).
Данные отличия (embedded linux вместо bare metal и раздельные датчики вместо одного) могут создать дополнительные проблемы. Возможно, потребуется PREEMT_RT.
Также необходимо провести интеграцию с одной из существующих GUI-утилит для калибровки ориентации и отслеживания положения контроллера относительно осей координат в режиме реального времени: 1, 2, 3. Либо любой другой утилитой на усмотрение разработчика. По любому интерфейсу (uart, usb, ethernet, wifi) на усмотрение разработчика.