STM32: Refactored driver; NuttX CPU ussage dropped to 7%