Сделал свой функциональны блок на ST на основе кода из справки, проблему решил.
Спасибо