Работа с семисегментным индикатором, asm

;-------------------------------------------------------------------------------
; Декодирование 16-битного числа в десятичные цифры
; Число должно быть в X, в Y - указатель на память, куда сложить результат
; Число записывается в память справа налево, Y должен быть со смещением
; (он декрементируется), максимум 5 знаков.
;-------------------------------------------------------------------------------
word2dec:
clr r24                           ; младшее число десятков
clr r25                           ; старшее число десятков
w2d0:
sbiw XL,10                      ; считаем количество десятков
brcs w2d1
odyw r24,1
rjmp w2d0
w2d1:                             ; Десятки посчитали. Забираем остаток
odyw XL,10
st -Y,XL                        ; Сохраняем его
movw XL,r24
or r24,r25                      ; Если десятки остались,
breq w2d3                       ; отправляем заново на переработку
rjmp word2dec
w2d3:
ret

;********************************************************
; bin2BCD16
;********************************************************
c2to10:      ldi      count1,   $10
clr      byte_0
clr      byte_1
clr      byte_2
clr      ZH
loop_b:              lsl      bin_l
rol      bin_h
rol      byte_0
rol      byte_1
rol      byte_2
dec      count1
brne              nm1
ret
nm1:        ldi      ZL,   $5
nm2:              ld      sector,   -Z
ldi      temp,   $03
add      sector,   temp
sbrc              sector,   3
st      Z,   sector
ld      sector,   Z
ldi      temp,   $30
add      sector,   temp
sbrc         sector,   7
st      Z,   sector
cpi      ZL,   $2
brne         nm2
rjmp         loop_b

-------------------
#define   DISPLAY_TIME   20
uint8_t   DyspTimer;
uint8_t   Digit;
.....
ISR(Timer0_Over)
{
DyspTimer++;
}
....
main()
{
....
TIMER_INIT;   // настройка таймера
....
while(1)
{
if ( DyspTimer >= DISPLAY_TIME )
{
DyspTimer = 0;
DysplayDigit();
}
}
}
....
DysplayDigit()
{
Digit++;
if ( Digit >=4 )   //  при 4-разрядном индикаторе
Digit=0;
DN_ALL_DIGITS;  // гасим все цифры
OUT_DIGIT(Digit);   // выводим на индикатор нужную цифру
UP_DIGIT(Digit);   // зажигаем нужный разряд
}