Большое спасибо за подсказку. Блок не стал делать, так на языке ST и переписал.