В оскат библиотеке на языке ST есть такая функция, сравните с ней то что написали в ОЛ