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

 #define BREAKER_COUNT_MIN (1E6/(MICROSECS_PULSE * 300))
 #define COUNT_FROM_RPM(RPM) ((1E6/(MICROSECS_PULSE * 30 / (RPM ) )))
-#define SAMPLE_BUFF_SIZE 256
+#define SAMPLE_BUFF_SIZE 16
 uint16_t nominal = 0;
 uint16_t halfRot;
 uint16_t phase10 = 100; // 10 degrees
 volatile uint16_t sampleRefCount = 0;
 uint16_t outSampleRefCount = 0;
-volatile uint16_t sampleRefBuff[SAMPLE_BUFF_SIZE];
 volatile uint16_t  sampleVar;
 volatile uint16_t sampleRef;
 volatile uint16_t  sampleDiffBuff[SAMPLE_BUFF_SIZE];
+volatile uint16_t samplePeriodBuff[SAMPLE_BUFF_SIZE];
+volatile uint8_t   refCountBuff[SAMPLE_BUFF_SIZE];
+volatile uint8_t  varCountBuff[SAMPLE_BUFF_SIZE];
+volatile uint8_t haveSlowPulse = 0;
 uint16_t rpm;
 uint16_t count;
 uint16_t delta;
+{
         return  count;
+}
+uint8_t slowPulse(void)
+{
+        return haveSlowPulse;
+}
-// waits for ignition pulse , debounces readings,
+// waits for ignition pulse reading in the buffer, returns
-// returns the pulse time, skips debounce time
+// the sample index of the next sample.
 uint16_t getNextRefPulseIndex(void)
+{
         while (outSampleRefCount == sampleRefCount)
                 chThdSleep(10);
         outSampleRefCount = 0;
     return sampleIndex;
+}
 void processNextPulse(uint16_t index)
+{
     // scale it up by 32
         static int32_t periodEstimate = 2000 * 256 ;
         // at this point we should try to phase lock
-     static signed pdLast = 0;
         uint16_t sampleDiff = sampleDiffBuff[index];
+        signed pd = 0;
+        bool lock = false;
+// basically locked
+        if(refCountBuff[index]==1 && varCountBuff[index]==1 )
+        {
+                lock = true;
+       pd = (sampleDiff <  32768 ? sampleDiff : sampleDiff - 65536L) ;
+        }
+        else if (refCountBuff[index] > 1)
+        {
+                pd = (refCountBuff [index]*32768L);
+        }
+        else if (varCountBuff[index] > 1)
+        {
+                pd = (-varCountBuff[index] * 32768L);
+        }
+    float delta_phi = pd/ 2000;
-    signed pd = (sampleDiff <  32768 ? sampleDiff : sampleDiff - 65536L) ;
+    // phase detector returns +/
-    static float phase_offset = 0;
     float const wn = 0.01f;
     float const zeta = 0.707f;
-    float const K = 100;
+    float const K = 1;
     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.0f, v1 = 0.0f, v2 = 0.0f;
+    static float v0=0, v1 = 0, v2 = 0 ;
     static float phi_hat = 0.0f;
-    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;
+    phi_hat = v0 * b0 + v1 * b1 + v2 * b2 ;
+    delta = phi_hat;
+    // 6.283 = 1.0 Hz
+    // 62.2 = 10Hz
+    // decide on whether to go for forcing loop or to track
+    int32_t arr;
+   //   if(lock)
-    uint16_t arr = phi_hat < 100 ? 100 : phi_hat;
+       arr = (6283000L/MICROSECS_PULSE)/ phi_hat;
+    if(arr > 5000)
+        arr = 5000;
+    if(arr < 500)
+        arr = 500;
-    // compute period error
+    // compute period error]
       // periodErr +=   periodEstimate - (arr * 256);
      count = arr;
-     delta = pd;
     TIM2->ARR = arr ;
     recalcPhase();
 // 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)
+        {
                 TIM3->SR &= ~TIM_SR_CC1IF;
                 uint16_t sample = TIM3->CCR1;
                 if(sample-lastSampleRef >  100 /*BREAKER_COUNT_MIN */)
+                {
                         sampleRef = sample;
                         ++refCount;
+                }
+        samplePeriod = sample-lastSampleRef;
+        chDbgCheck(samplePeriod != 65535);
-                lastSampleRef = sample;
+        lastSampleRef = sample;
+        }
         if(stat & TIM_SR_CC2IF)
+        {
                 TIM3->SR &= ~TIM_SR_CC2IF;
                  sampleVar  = TIM3->CCR2;
     if(refCount != 0 && varCount != 0  ) /*we have an R,V pair  */
+    {
+//
+    refCountBuff[sampleRefCount] = refCount;
+    varCountBuff[sampleRefCount] = varCount;
+     haveSlowPulse = (varCount > 20);
+     sampleDiffBuff[sampleRefCount] = sampleRef - sampleVar;
-     if(refCount < varCount)
-         sampleDiffBuff[sampleRefCount] = 32767; // indicate +ve max phase
+     samplePeriodBuff[sampleRefCount] = samplePeriod;
-     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 13 → 14