мой код на ST, в общем должен Вас устроить.
Только поменять переменные.
Ну или как знаете.....