Subversion Repositories chibiosIgnition

/*

 * timer2.c

 *

 *  Created on: 2 Apr 2018

 *      Author: Mike

 */

#include "ch.h"  // needs for all ChibiOS programs

#include "hal.h" // hardware abstraction layer header

#include "timer2.h"

#define  MICROSECS_PULSE 10

// 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 .

// 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 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  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];

int gainControl = 1000 ;

volatile uint8_t haveSlowPulse = 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

    // 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,

                          4);

    TIM3->DIER |= TIM_DIER_CC1IE  | TIM_DIER_CC2IE;

}

void recalcPhase(void)

{

        nominal = halfRot * (long) (phase10)/ 1800;

}

void adjustRPM(void)

{

        if(rpm < 600)

                rpm = 600;

        if(rpm >  5000)

                rpm = 5000;

}

uint16_t setRPM(uint16_t rpm_ )

{

        if(rpm_ >= 600 && rpm_ < 6000)

        {

          rpm = rpm_;

          adjustRPM();

        }

          return halfRot;

}

uint16_t getRPM(void)

{

        return rpm;

}

signed getDelta(void)

{

        return delta;

}

signed getCount(void)

{

        return  count;

}

void setGain(int gain)

{

        gainControl = gain;

}

uint8_t slowPulse(void)

{

        return haveSlowPulse;

}

// waits for ignition pulse reading in the buffer, returns

// the sample index of the next sample.

uint16_t getNextRefPulseIndex(void)

{

        while (outSampleRefCount == sampleRefCount)

                chThdSleep(10);

        uint16_t sampleIndex = outSampleRefCount;

    if(++outSampleRefCount == SAMPLE_BUFF_SIZE)

        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

        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/ (gainControl * 1.0);

    delta = delta_phi;

    // phase detector returns +/

    float const wn = 0.03f;

    float const zeta = 0.707f;

    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, v1 = 0, v2 = 0 ;

    static float phi_hat = 0.0f;

    v2=v1; v1=v0;

    v0 = delta_phi -v1 *a1 -v2 *a2;

    phi_hat = v0 * b0 + v1 * b1 + v2 * b2 ;

    // 6.283 = 1.0 Hz

    // 62.2 = 10Hz

    // decide on whether to go for forcing loop or to track

    int32_t arr;

   //   if(lock)

       arr = (6283000L/MICROSECS_PULSE)/ phi_hat;

    if(arr > 5000)

        arr = 5000;

    if(arr < 500)

        arr = 500;

    // compute period error]

      // periodErr +=   periodEstimate - (arr * 256);

     count = arr;

    TIM2->ARR = arr ;

    recalcPhase();

// calculate RPM

    float nomRPM = 30E6 / (MICROSECS_PULSE * (periodEstimate/256));

        rpm =  nomRPM ;

//      rpm += delta / 256;

        adjustRPM();

    }

// set the timing advance from reference to

void setAdvance(int16_t deg10)

{

    phase10 = deg10;

    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;

        }

        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  */

    {

//

    refCountBuff[sampleRefCount] = refCount;

    varCountBuff[sampleRefCount] = varCount;

    haveSlowPulse = (varCount > 20);

     sampleDiffBuff[sampleRefCount] = sampleRef - sampleVar;

     samplePeriodBuff[sampleRefCount] = samplePeriod;

            if(++sampleRefCount == SAMPLE_BUFF_SIZE)

                  sampleRefCount = 0;

   refCount = 0;

   varCount = 0;

   }

}