Pomiar odległości AVR

Question

Mam czujnik odległości US-015. Pin echo podłączony do INT0 i INT1. Kod:

#define F_CPU 16000000UL

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdio.h>

void print_char(char letter)
{
	if(	bit_is_set(UCSRA, UDRE)	)
	{
		_delay_ms(10);
		UDR=letter;
		loop_until_bit_is_set(UCSRA, TXC);
	}
}

volatile unsigned long long time=0;
volatile long double czas=0;
volatile long double cm=0.0;
volatile int buf2=0;

int main(void)
{
    DDRD |= (1<<PD7);
    DDRD &= ~(1<<PD2);
    DDRD &= ~(1<<PD3);

    UBRRL = 103; //9600 16mhz
    UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE);
    UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);

    TCCR0 = (1<<CS01);
    TIMSK = (1<<TOIE0);

    MCUCR |= (1<<ISC01)|(1<<ISC00)|(1<<ISC11);
    GICR |= (1<<INT0) | (1<<INT1);

    sei();

    while(1);
}

ISR(USART_RXC_vect)
{
    char buf=UDR;
	
    PORTD ^= (1<<PD7);
    _delay_us(10);
    PORTD ^= (1<<PD7);
}

ISR(TIMER0_OVF_vect)
{
    TCNT0=0;
    time++;
}

ISR(INT0_vect)
{
    TCNT0=0;
    time=0;
}

ISR(INT1_vect)
{
    buf2=TCNT0;
    czas=time;

    TIMSK ^= (1<<TOIE0);
    TCNT0=0;
    time=0;

     //1 cykl zegara to 0.0625 uS
    //256 cykli = 1 czas = 15.9375 uS

    czas*=15.9375;
    buf2*=0.0625;
    czas+=buf2;

    cm=czas/58.8235;

    char dys[32];

	dtostrf(cm, sizeof(long double), 8, dys);

	int ile=5;
	if(cm>=10) ile++;
	if(cm>=100) ile++;

	for(int i=0; i<ile; i++) print_char(dys[i]);
}

Wszystko działa, oprócz tego, że nie wyświetla się poprawna odległość. Próbowałem różnych "wzorów", ale żaden nie działa. Co jest nie tak?