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

 // 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 .
 // the TIM2 counter counts in 10uS increments,
 #define BREAKER_COUNT_MIN (1E6/(MICROSECS_PULSE * 300))
+#define COUNT_FROM_RPM(RPM) ((1E6/(MICROSECS_PULSE * 30 / (RPM ) )))
 #define SAMPLE_BUFF_SIZE 256
+uint16_t nominal = 0;
 uint16_t halfRot;
-uint16_t nominal  = 0;
 uint16_t phase10 = 100; // 10 degrees
-volatile uint16_t sampleCount = 0;
+volatile uint16_t sampleRefCount = 0;
-uint16_t outSampleCount = 0;
+uint16_t outSampleRefCount = 0;
-volatile uint16_t sampleBuff[SAMPLE_BUFF_SIZE];
+volatile uint16_t sampleRefBuff[SAMPLE_BUFF_SIZE];
-typedef enum { WAIT_GAP, SKIP_BOUNCE, HAVE_SAMPLE } sampleState_t ;
-sampleState_t  sampleState = WAIT_GAP;
+volatile uint16_t  sampleVar;
-// difference between samples
-volatile uint16_t deltaTime;
+volatile uint16_t sampleRef;
-static signed pdI = 0;
+volatile uint16_t  sampleDiffBuff[SAMPLE_BUFF_SIZE];
 uint16_t rpm;
+uint16_t count;
+uint16_t delta;
 void initTimer2()
+{
         rccEnableTIM2(FALSE);
         rccResetTIM2();
         TIM3->ARR = 0xFFFF;
         TIM3->CCMR1 = TIM_CCMR1_CC1S_0 /* | TIM_CCMR1_IC1F_0 | TIM_CCMR1_IC1F_1 | TIM_CCMR1_IC1F_2 */ ;  // filter 16, input
-        TIM3->CCER = TIM_CCER_CC1E;
-    // link TIM3 ITR1 to TIM2 reload
+    // link TIM3 ITR2 to TIM2 reload
-    // use CCR3
+    // 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->CCMR2 = TIM_CCMR2_CC3S_1 | TIM_CCMR2_CC3S_0 ; //  The
+    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 ;
+    TIM3->DIER |= TIM_DIER_CC1IE  | TIM_DIER_CC2IE;
+}
 void recalcPhase(void)
+{
 uint16_t getRPM(void)
+{
         return rpm;
+}
-uint16_t getDelta(void)
+unsigned getDelta(void)
+{
-        return pdI&0xFFFF;
+        return delta;
+}
-uint16_t wrapIndex(uint16_t index)
+unsigned getCount(void)
+{
-        if (index >= SAMPLE_BUFF_SIZE)
-                index -= SAMPLE_BUFF_SIZE;
-    return index;
+        return  count;
-}
-// allows for wrapping
-uint16_t getSampleBuff(uint16_t index)
-{
-        chSysLock();
-        return sampleBuff[wrapIndex(index)];
-        chSysUnlock();
+}
 // waits for ignition pulse , debounces readings,
 // returns the pulse time, skips debounce time
-uint16_t getNextPulse(void)
+uint16_t getNextRefPulseIndex(void)
+{
-        static uint16_t lastSampleIndex = 0;
-        while(1)
-        {
-                        while (outSampleCount == sampleCount)
+        while (outSampleRefCount == sampleRefCount)
-                                chThdSleep(10);
+                chThdSleep(10);
-                        uint16_t thisTime  = getSampleBuff(outSampleCount);
+        uint16_t sampleIndex = outSampleRefCount;
-                        outSampleCount = wrapIndex(outSampleCount + 1);
-                        while (outSampleCount == sampleCount)
+    if(++outSampleRefCount == SAMPLE_BUFF_SIZE)
-                                chThdSleep(10);
-                        uint16_t nextTime = getSampleBuff(outSampleCount);
-                        // calculate wrapped time delta : should be > than bounce time to allow
-                        uint16_t diffTime = nextTime - thisTime;
-                        if(diffTime > BREAKER_COUNT_MIN)
-                        {
-                                lastSampleIndex = outSampleCount;
+        outSampleRefCount = 0;
-                                return nextTime;
+    return sampleIndex;
-                        }
-        }
-  return 0;
+}
-void processNextPulse(uint16_t retVal)
+void processNextPulse(uint16_t index)
+{
-        static uint32_t VperiodAccumulator = 0;
     // scale it up by 32
-        static uint32_t periodEstimate = 2000 * 256 ;
+        static int32_t periodEstimate = 2000 * 256 ;
-        static uint16_t lastVal = 0;
         // at this point we should try to phase lock
-    deltaTime = retVal - lastVal;
+     static signed pdLast = 0;
+        uint16_t sampleDiff = sampleDiffBuff[index];
+    signed pd = (sampleDiff <  32768 ? sampleDiff : sampleDiff - 65536L) ;
-    if(deltaTime > 10000)
-    {
-        __asm(" BKPT #0");
-    }
-    lastVal = retVal;
-    // look at the values and try to pull them togwther
+    static float phase_offset = 0;
+    float const wn = 0.01f;
+    float const zeta = 0.707f;
+    float const K = 100;
-    // accumulate phase
+    float const t1 = K/(wn*wn);
-    VperiodAccumulator += periodEstimate;
+    float const t2 = 2 * zeta/wn;
+    float const  b0 = (4*K/t1)*(1.+t2/2.0f);
-    VperiodAccumulator &= 0xFFFFFFF;
+    float const b1 = (8*K / t1);
+    float const b2 = (4*K/t1)*(1.-t2/2.0f);
-    uint16_t accum_low = (VperiodAccumulator) >> 8;
+    float const a1 = -2.0f;
-#define WINDOW 1000
+    float const a2 = 1.0f;
-#define LIMIT 2048
+    static float v0=0.0f, v1 = 0.0f, v2 = 0.0f;
-#define STEP 32
-    uint16_t diff = accum_low - retVal;
-    if(diff < WINDOW && diff != 0)
+    static float phi_hat = 0.0f;
-        pdI += diff/10;
-    if(diff > 65536-WINDOW )
-        pdI -= (65536-diff)/10;
-    if(pdI > LIMIT)
-        pdI = LIMIT;
-    if(pdI < -(LIMIT))
-        pdI = -(LIMIT);
+    float delta_phi = pd/ 32768.0;
+    v2=v1; v1=v0;
+    v0 = delta_phi -v1 *a1 -v2 *a2;
+    phi_hat = v0 * b0 + v1 * b1 + v2 * b2;
-    signed pd = (signed)(periodEstimate)- (signed)(deltaTime*256) + pdI;
+    uint16_t arr = phi_hat < 100 ? 100 : phi_hat;
+    // compute period error
-    periodEstimate -= pd / 100;
+      // periodErr +=   periodEstimate - (arr * 256);
+     count = arr;
+     delta = pd;
-    TIM2->ARR = (periodEstimate+128)/256 ;
+    TIM2->ARR = arr ;
     recalcPhase();
 // calculate RPM
+}
 // timer 3 interrupt
 void VectorB4(void)
+{
+    static uint16_t lastSampleRef = 0;
+    static uint8_t refCount = 0;
+        static uint8_t varCount = 0;
         uint16_t stat = TIM3->SR;
         if(stat & TIM_SR_CC1IF)
+        {
                 TIM3->SR &= ~TIM_SR_CC1IF;
-            uint16_t sample = TIM3->CCR1;
+                uint16_t sample = TIM3->CCR1;
-            sampleBuff[sampleCount++] = sample;
+                if(sample-lastSampleRef >  100 /*BREAKER_COUNT_MIN */)
+                {
-            if (sampleCount >= SAMPLE_BUFF_SIZE)
+                        sampleRef = sample;
+                        ++refCount;
+                }
-                sampleCount = 0;
+                lastSampleRef = sample;
+        }
+        if(stat & TIM_SR_CC2IF)
+        {
+                TIM3->SR &= ~TIM_SR_CC2IF;
+                 sampleVar  = TIM3->CCR2;
+                 ++varCount;
+        }
+    if(refCount != 0 && varCount != 0  ) /*we have an R,V pair  */
+    {
+     if(refCount < varCount)
+         sampleDiffBuff[sampleRefCount] = 32767; // indicate +ve max phase
+     if(refCount > varCount )
+         sampleDiffBuff[sampleRefCount] = 65536-32767 ; // indicate -ve max phase
+     if(varCount == refCount)
+            sampleDiffBuff[sampleRefCount] = sampleRef - sampleVar;
+     if(sampleRef)
+         sampleRefBuff[sampleRefCount] = sampleRef;
+            if(++sampleRefCount == SAMPLE_BUFF_SIZE)
+                  sampleRefCount = 0;
+   refCount = 0;
+   varCount = 0;
+   }
+}

Subversion Repositories chibiosIgnition

(root)/trunk/src/timer2.c – Rev 12 → 13