Velocímetro usando CCP

Hice un velocímetro utilizando el pin CCP como entrada. funciona. pero tengo algun problema la velocidad cambia con frecuencia dentro de un rango, incluso usando una frecuencia estática (cuando uso una frecuencia estática, debería mostrar un valor fijo). Utilicé la velocidad promedio (suma pocos tiempos de pulso y obtuve el promedio). luego ocurrió otro problema (cuando la velocidad es lenta, el cambio de velocidad es un retraso). ¿Qué es lo malo? Mi código completo está abajo.

Por favor, ¿alguien puede ayudarme?

//PIC - 16F648A Lang.- MikroC clock- Internal clock 4MHz
unsigned int speed;
unsigned short one_digit,ten_digit,hun_digit,overflow;
unsigned long one_pulse_time;
bit time_over,speed_change;
void calc_speed();
void ssdecode(int i);
void main() {
  CMCON = 0b00000111;
  TRISA = 0b0010000;
  TRISB = 0b0001000;
  INTCON = 0b11000000;
  T1CON = 0b00110011;        // Prescaler 1:8
  PIE1 = 0b00000101;
  PIR1 = 0b00000000;
  CCP1CON = 0b00000100;
  PIE1.CCP1IE  = 1 ;                  // enable CCP1 Interrupt
  PIR1.CCP1IF  = 0 ;                    // Clear CCP1 INTERRUPT Flag
  TMR1H        = 0 ;                    // reset Timer1
  TMR1L        = 0  ;
  PIR1.TMR1IF  = 0  ;                   // Timer1 Overflow Flag Cleared
  T1CON.TMR1ON = 1 ;
  speed_change = 0;
  one_pulse_time = 0;
  speed = 0;
  overflow = 0;
  do {
     if(speed_change==1) calc_speed();
     ssdecode(one_digit); //Display one's digit
     PORTA.F2 = 1;
     Delay_ms(2);
     PORTA.F2 = 0;
     ssdecode(ten_digit); //Display ten's digit
     PORTA.F3 = 1;
     Delay_ms(2);
     PORTA.F3 = 0;
     ssdecode(hun_digit); //Display hundred's digit
     PORTA.F4 = 1;
     Delay_ms(2);
     PORTA.F4 = 0;
  }while(1);
}
void interrupt()  {
  if(PIR1.TMR1IF)  {
     overflow++;
     PIR1.TMR1IF=0;
  }
  if (PIR1.CCP1IF) {// Check for Timer 0 interrupt
     if(overflow < 2) {
       one_pulse_time = overflow*65536;
       ccp1con = 0b00000000;
       one_pulse_time += CCPR1H*256 + CCPR1L;
       ccp1con = 0b00000100;
       time_over = 0;
     }
     else  time_over = 1;'

     overflow = 0;
     speed_change = 1;
     PIR1.TMR1IF=0;
     PIR1.CCP1IF = 0; // Clear RB0 interrupt flag
'  }
}'
'
void calc_speed() {
   if (time_over==0 && one_pulse_time > 0 ) {
      speed=(3600000*1.7)/(one_pulse_time*8); //   3.6*1000000 = kmh  , 1.7m = wheel circumference , (one_pulse_time*Prescaler)
      one_digit = speed%10;
      speed=speed/10;
      ten_digit = speed%10;
      hun_digit = speed/10;
      speed_change=0;
      one_pulse_time = 0;
   }
   else speed=0;
}
void ssdecode(int i)
{
   switch (i)
   {
      case 0: PORTB = 0b01111110; break;
      case 1: PORTB = 0b00001100; break;
      case 2: PORTB = 0b10110110; break;
      case 3: PORTB = 0b10011110; break;
      case 4: PORTB = 0b11001100; break;
      case 5: PORTB = 0b11011010; break;
      case 6: PORTB = 0b11111010; break;
      case 7: PORTB = 0b00001110; break;
      case 8: PORTB = 0b11111110; break;
      case 9: PORTB = 0b11011110; break;
   }
}

actualizacion :

Lo corregí. Está funcionando. Yo corrijo así,

Pero tengo otro pequeño problema. El 7SD siempre está temblando y parpadea. Encontré la falla. se tarda 3 ms (aproximadamente) para calc_Speed. ¿Cómo puedo arreglarlo? ayúdame.

void interrupt()  {
  if(PIR1.TMR1IF)  {
     overflow++;
     PIR1.TMR1IF=0;
  }
  if(overflow > 2) {
     time_over = 1;
     one_digit = 0; ten_digit = 0; hun_digit = 0;
  }
  if (PIR1.CCP1IF) {// Check for Timer 0 interrupt
     if(overflow < 3) {
       one_pulse_time_of = overflow*65536;
       PIE1.CCP1IE = 0;
       CCP1CON = 0b00000000;
       one_pulse_time = one_pulse_time_of + (CCPR1H*256 + CCPR1L);
       CCP1CON = 0b00000101;
       PIE1.CCP1IE = 1;
       time_over = 0;
     }
     else  time_over = 1;
     T1CON.TMR1ON = 0 ;
     CCP1CON = 0b00000000;
     CCPR1H=0x00;
     CCPR1L=0x00;
     CCP1CON = 0b00000101;
     TMR1H = 0x00 ;                   // Set initial value for the timer TMR1 for maximum counts available
     TMR1L = 0x00 ;
     overflow = 0;
     speed_change = 1;
     T1CON.TMR1ON = 1 ;
     PIR1.TMR1IF=0;
     PIR1.CCP1IF = 0; // Clear RB0 interrupt flag
  }
}
void calc_speed() {
   if ((time_over == 0) && (one_pulse_time > 0)) {
      speed=(3600000*1.8)/(one_pulse_time*8*5);       //  1.8m = wheel circumference , (one_pulse_time*Prescaler*pulse_per_revolution)
      one_digit = speed%10;
      speed=speed/10;
      ten_digit = speed%10;
      hun_digit = speed/10;
      speed_change=0;
      one_pulse_time = 0;
   }
   else {
      one_digit = 0; ten_digit = 0; hun_digit = 0;
   }
}