CC2541 測量方波寬度 (Value不準)

我是從simplePeripheral修改，並在其中一個event使用pin 2_1 去trigger一個外部的超音波感測器

超音波會從他的echo pin回傳一個方波到CC2541其中一個I/O pin

我是用Interrupt(rising edge & falling edge)跟Timer1來測量

Triggle 跟 超音波感測回傳的方波訊號都很正常 (用示波器測量的)

但計算出的距離就是不太對

另外我有一個EventB也會用到Timer(收集MPU9250的raw data)，但解析度不一樣

每次都會重新設定 T1CTL 的值

EventB 的值都蠻正常的

不好意思麻煩各位 謝謝!!!

以下是我的code

void SimpleBLEPeripheral_Init( uint8 task_id )
{
 //default settings...

 //my registers settings

  //ultrasonic trigle pin
  P2DIR |= 0x02;
  P2_1 = 1;// output pin
  //ultrasonic echo pin 
  P0DIR &= ~0x04;
  P0SEL &= ~0x04; 
  //P0_2 interrupt setting
  IEN1 |= 0x20;
  P0IEN |= 0x04; //p0_2
  P0IFG &= ~0x00;  
  PICTL &= ~0x01;//P0 rising edge
  IRCON &= ~0x00;
  EA = 1;   
  //timer setting
  T1CTL = 0x09; 
  T1IE = 0;
}


uint16 SimpleBLEPeripheral_ProcessEvent( uint8 task_id, uint16 events )
{
   if ( events & TRIGGLESONIC_EVT )
  { 
    T1CTL = 0x05; 
    PICTL &= ~0x01;
   
    triggleUltrasonic();
    
    ultraperiod = (float)ult_end/4000.0f;     
    
    T1CTL = 0x09; 
  
    IMUProfile_SetParameter(UNDECIDED, sizeof(ultraperiod) ,&ultraperiod);  
  
    return (events ^ TRIGGLESONIC_EVT);
  }
} 



#pragma vector = P0INT_VECTOR 
   __interrupt void P0INT(void)
{      
  
  P0IFG = 0;   
  P0IF = 0;
  PICTL ^= 0x01;
  
  if((P0IFG & 0x04)== 4)
  {   
    if(PICTL & 0x01 == 1)
    {
      ult_end = T1CNTL;
      ult_end |= T1CNTH<<8;
      
      P0_4 ^= 1; // LED for test
    }
    else
    {
      T1CNTH = 0;
      T1CNTL = 0;
      
      P0_5 ^= 1; // LED for test

    }    
  }
  
}