Input Capture Unit für PWM Duty Cycle

Hallo,
ich möchte den Timer Capture Interrupt eines ATmega328P nutzen, um 
später den Duty-Cycle eines 4 kHz PWM-Signals (Temperatursensor 
SMT16030) berechnen zu könnnen. Ich weiß, dass der Duty-Cycle bei ca. 43 
% liegen muss.
Mit der if-Abfrage in der Endlosschleife möchte ich testen, ob der 
Interrupt überhaupt sinnvolle Werte zurückgibt. Leider ist dies nicht 
der Fall. Manchmal ist die Abfrage positiv, manchmal negativ. Eigentlich 
sollte sie ja immer stimmen.
Kann jemand den/die Fehler in meinem Code erkennen, oder Tipps geben?
Gruß Tim

#define F_CPU 8000000

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdint.h>

/* Macros */
#define ICP_SENSE  ICES1
#define ICP_START_SENSE (1 << ICES1)

/* Typedefs */
typedef unsigned int icp_timer_t;

/* Variables */
volatile icp_timer_t icp_start_time, icp_stop_time;
volatile icp_timer_t icp_period;
volatile icp_timer_t icp_pulsewidth;
uint8_t ts;

ISR(TIMER1_CAPT_vect)
{
  icp_timer_t icr;
  icp_timer_t delta;
  unsigned char tccr1b;

  /* Capture ICR */
  icr = ICR1;

  /* Reverse the sense */
  tccr1b = TCCR1B;
  TCCR1B = tccr1b  ^ (1 << ICP_SENSE);

  if ((tccr1b & (1 << ICP_SENSE)) == ICP_START_SENSE)
  {
    icp_period = icr - icp_start_time;
    delta = icp_stop_time - icp_start_time;
    icp_start_time = icr;
    icp_pulsewidth = delta;
  }
  else
  {
    icp_stop_time = icr;
  }
}

int main(void)
{
  /* Datenrichtungen */
  DDRB = 0x00;
  PORTB = 0xFF - (1 << PB0);
  DDRD = 0xFF;

  /* Timer 1 initialisieren */
  TIMSK1 = 1 << ICIE1;
  TCCR1A = 0;
  TCCR1B = ((0 << CS12) | (0 << CS11) | (1 << CS10));
  TCCR1B = 1 << ICES1;
  TCCR1B = 1 << ICNC1;

  sei();
    while (1)
    {
    if (icp_period != icp_pulsewidth)
    {
      ts = PINB;
      PORTD = ts;
    }
  }
  return 0;
}