WebSVN – chibiosIgnition – Diff – /trunk/src/timer2.c

 #include "ch.h"  // needs for all ChibiOS programs
 #include "hal.h" // hardware abstraction layer header
 #include "timer2.h"
-#define  MICROSECS_PULSE 10
+#define  MICROSECS_PULSE 1
 // with a dwell angle of 45 degrees , 4 cylinders and a maximum RPM of 5000
 // freq = 5000/60 * 2 = 166Hz. Because the breaker might bounce , we accept the
 // first pulse longer than 1/300 of a second as being a proper closure .
 uint16_t nominal = 0;
 uint16_t halfRot;
 uint16_t phase10 = 100; // 10 degrees
-volatile uint16_t sampleRefCount = 0;
-volatile uint16_t  sampleVar;
+volatile uint16_t sampleVar;
 volatile uint16_t sampleRef;
+volatile uint16_t lastSampleRef = 0;
-int gainControl = 1000 ;
+volatile uint8_t refCount = 0;
-volatile uint8_t haveSlowPulse = 0;
+volatile uint8_t varCount = 0;
-uint16_t rpm;
-signed count;
-signed delta;
-void initTimer2()
-{
-        rccEnableTIM2(FALSE);
-        rccResetTIM2();
-        TIM2->PSC = 72*MICROSECS_PULSE;
-        TIM2->ARR = 60000;
-        TIM2->CR1 = ~TIM_CR1_CKD  & (TIM_CR1_CEN |
-                        TIM_CR1_ARPE );
-        /// pulse width 200 uS
-        TIM2->CCR1 = 200/MICROSECS_PULSE;
-    TIM2->CCER =  TIM_CCER_CC1E | TIM_CCER_CC1P  ; //enabled and active high
-    TIM2->CCMR1 = TIM_CCMR1_OC1M_0 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 |
-                           TIM_CCMR1_OC1PE ;
-    TIM2->CR2 = TIM_CR2_MMS_1 ; // trigger out is 010 = update
+volatile uint16_t samplePeriod = 0 ;
+static signed phaseSamp = 0;
+static uint8_t validPhaseSamp = 0;
-    // change the TIM2 CC2 to TIM3 CC1
-        rccEnableTIM3(FALSE);
-        rccResetTIM3();
-        // TIM3 on the PA6 ... pins : remap code 00
-        AFIO->MAPR &= ~ AFIO_MAPR_TIM3_REMAP;
-        TIM3->PSC = 72*MICROSECS_PULSE;
-        TIM3->ARR = 0xFFFF;
-        TIM3->CCMR1 = TIM_CCMR1_CC1S_0 /* | TIM_CCMR1_IC1F_0 | TIM_CCMR1_IC1F_1 | TIM_CCMR1_IC1F_2 */ ;  // filter 16, input
-    // link TIM3 ITR2 to TIM2 reload
-    // use TS = 001 to make TRC from Tim2 TRIGGER
-        TIM3->SMCR  &= ~(TIM_SMCR_TS_Msk );
-        TIM3->SMCR  |=  TIM_SMCR_TS_0; // select ITR2 as trigger source TRC
-    TIM3->CCMR1 |=  TIM_CCMR1_CC2S_1 |  TIM_CCMR1_CC2S_0 ; //  The CC2S bits are 11, use TRC
-        TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;
-        TIM3->CR1 = ~TIM_CR1_CKD  & (TIM_CR1_CEN | TIM_CR1_ARPE );
-    nvicEnableVector(TIM3_IRQn,
-);
+int gainControl = 1000 ;
+uint16_t rpm;
+signed count;
+signed delta;
-    TIM3->DIER |= TIM_DIER_CC1IE  | TIM_DIER_CC2IE;
-}
 void recalcPhase(void)
+{
         nominal = halfRot * (long) (phase10)/ 1800;
+{
         gainControl = gain;
+}
-uint8_t slowPulse(void)
-{
-        return haveSlowPulse;
-}
-void processPhase (int refCount,int varCount, int diff)
+void processPhase ( void )
+{
+    // lpcl
+        chSysLock();
+        signed pd;
+        pd = 0;
-    // scale it up by 32
+static  unsigned sp = 0;
+        if(validPhaseSamp)
+        {
-        // at this point we should try to phase lock
+         pd =   phaseSamp;
+     validPhaseSamp = 0;
+     delta = phaseSamp;
-        signed pd = 0;
-        bool lock = false;
-// basically locked
-        if(refCount==1 && varCount==1 )
-        {
-                lock = true;
-       pd = (diff <  32768 ? diff : diff - 65536L) ;
-        }
-        // frequency detector
+     sp = samplePeriod;
-        else if (refCount > 1)
-        {
-                pd = (refCount*32768L);
-        }
-        else if (varCount > 1)
-        {
-                pd = (-varCount * 32768L);
+        }
-    float delta_phi = pd/ (gainControl * 1.0);
-    delta = pd;
+     chSysUnlock();
+     static int sampleAverage = 0;
+     static int phaseAverage  = 0;
-    // phase detector returns +/
+     const int freqScale = 40;
+     const int phaseScale = 200;
+     sampleAverage = sampleAverage + (sp - sampleAverage/freqScale);
-    float const wn = 0.01;
-    float const zeta = 0.707f; // was 0.707
-    float const K = 1000;
-    float const t1 = K/(wn*wn);
-    float const t2 = 2 * zeta/wn;
-    float const  b0 = (4*K/t1)*(1.+t2/2.0f);
-    float const b1 = (8*K / t1);
-    float const b2 = (4*K/t1)*(1.-t2/2.0f);
-    float const a1 = -2.0f;
-    float const a2 = 1.0f;
-    static float v0=0, v1 = 0, v2 = 0 ;
-    static float phi_hat = 100.0f;
+     int32_t arr;
+   //   if(lock)
-    v2=v1; v1=v0;
-    v0 = delta_phi -v1 *a1 -v2 *a2;
+       int intSample = sampleAverage / freqScale;
+static unsigned strange = 0;
-    phi_hat = v0 * b0 + v1 * b1 + v2 * b2 ;
+       int deltaPd= pd/10;
+       if(pd < -2000 || pd > 2000)
+       {
-    // 6.283 = 1.0 Hz
+           strange++;
+       }
-    // 62.2 = 10Hz
-    // decide on whether to go for forcing loop or to track
+       arr =  intSample + deltaPd;
-    int32_t arr;
-   //   if(lock)
-       arr = (6283000L/MICROSECS_PULSE)/ (phi_hat+60);
-    if(arr > 20000)
+    if(arr > 65535)
-        arr = 20000;
+        arr = 65535;
-    if(arr < 100)
+    if(arr < 1000)
-        arr = 100;
+        arr = 1000;
      count = arr;
-    TIM2->ARR = arr ;
+    TIM2->ARR = arr -1;
     recalcPhase();
     float nomRPM = 30E6 / (MICROSECS_PULSE * arr);
     phase10 = deg10;
     recalcPhase();
+}
+// specialist timer setup :
+void initTimer2()
+{
+        rccEnableTIM2(FALSE);
+        rccResetTIM2();
+        TIM2->PSC = 72*MICROSECS_PULSE;
+        TIM2->ARR = 60000;
+        TIM2->CR1 = ~TIM_CR1_CKD  & (TIM_CR1_CEN |
+                        TIM_CR1_ARPE );
+        /// pulse width 200 uS
+        TIM2->CCR1 = 200/MICROSECS_PULSE;
+    TIM2->CCER =  TIM_CCER_CC1E | TIM_CCER_CC1P  ; //enabled and active high
+    TIM2->CCMR1 = TIM_CCMR1_OC1M_0 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 |
+                           TIM_CCMR1_OC1PE ;
+    TIM2->CR2 = TIM_CR2_MMS_1 ; // trigger out is 010 = update
+    // change the TIM2 CC2 to TIM3 CC1
+        rccEnableTIM3(FALSE);
+        rccResetTIM3();
+        // TIM3 on the PA6 ... pins : remap code 00
+        AFIO->MAPR &= ~ AFIO_MAPR_TIM3_REMAP;
+        TIM3->PSC = 72*MICROSECS_PULSE;
+        TIM3->ARR = 0xFFFF;
+        TIM3->CCMR1 = TIM_CCMR1_CC1S_0 /* | TIM_CCMR1_IC1F_0 | TIM_CCMR1_IC1F_1 | TIM_CCMR1_IC1F_2 */ ;  // filter 16, input
+    // link TIM3 ITR2 to TIM2 reload
+    // use TS = 001 to make TRC from Tim2 TRIGGER
+        TIM3->SMCR  &= ~(TIM_SMCR_TS_Msk );
+        TIM3->SMCR  |=  TIM_SMCR_TS_0; // select ITR2 as trigger source TRC
+    TIM3->CCMR1 |=  TIM_CCMR1_CC2S_1 |  TIM_CCMR1_CC2S_0 ; //  The CC2S bits are 11, use TRC
+        TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;
+        TIM3->CR1 = ~TIM_CR1_CKD  & (TIM_CR1_CEN | TIM_CR1_ARPE );
+    nvicEnableVector(TIM3_IRQn,
+);
+    TIM3->DIER |= TIM_DIER_CC1IE  | TIM_DIER_CC2IE;
+}
 // timer 3 interrupt
 void VectorB4(void)
+{
-    static uint16_t lastSampleRef = 0;
-    static uint8_t refCount = 0;
-        static uint8_t varCount = 0;
-        uint16_t samplePeriod;
-        uint16_t stat = TIM3->SR;
-        if(stat & TIM_SR_CC1IF)
+        if(TIM3->SR & TIM_SR_CC1IF)
+        {
-                TIM3->SR &= ~TIM_SR_CC1IF;
                 uint16_t sample = TIM3->CCR1;
-                if(sample-lastSampleRef >  100 /*BREAKER_COUNT_MIN */)
+//              if(sample-lastSampleRef >  1000 /*BREAKER_COUNT_MIN */)
+                {
+                samplePeriod = sample-sampleRef;
-                        sampleRef = sample;
+                sampleRef = sample;
                         ++refCount;
+                }
-        samplePeriod = sample-lastSampleRef;
+                lastSampleRef= sample;
-        chDbgCheck(samplePeriod != 65535);
-        lastSampleRef = sample;
+        }
-        if(stat & TIM_SR_CC2IF)
+        if(TIM3->SR & TIM_SR_CC2IF)
+        {
-                TIM3->SR &= ~TIM_SR_CC2IF;
                  sampleVar  = TIM3->CCR2;
                  ++varCount;
+        }
+    if(refCount == 1 && varCount == 1)
+    {
+        if(sampleRef == sampleVar)
+                phaseSamp = 0;
+        else
+        {
+        uint16_t refToVar = sampleRef - sampleVar;
+        uint16_t varToRef = sampleVar - sampleRef;
+        if(refToVar < 32768)
+                phaseSamp = refToVar;
+        else if(varToRef < 32768)
+                phaseSamp = -varToRef;
+        }
-    if(refCount != 0 && varCount != 0  ) /*we have an R,V pair  */
-    {
-//
-    processPhase (refCount,varCount,sampleRef-sampleVar);
-    haveSlowPulse = (varCount > 20);
+        validPhaseSamp = 1;
-   refCount = 0;
+        refCount=0;
-   varCount = 0;
+        varCount=0;
-   }
+    }
+    // frequency error, should deal with by direct period measurement
+    if(refCount > 1 || varCount > 1  )
+    {
+        refCount = 0;
+        varCount = 0;
+    }
+}

Subversion Repositories chibiosIgnition

(root)/trunk/src/timer2.c – Rev 18 → 19