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

// the TIM2 counter counts in 1uS increments,

#define BREAKER_COUNT_MIN (1E6/(MICROSECS_PULSE * 300))

#define COUNT_FROM_RPM(RPM) ((1E6/(MICROSECS_PULSE * 30 / (RPM ) )))

int16_t nominal = 0;

uint16_t halfRot;

int16_t phase10 = 100; // 10 degrees

volatile uint16_t sampleVar;

volatile uint16_t sampleRef;

volatile uint16_t lastSampleRef = 0;

volatile uint8_t refCount = 0;

volatile uint8_t varCount = 0;

volatile uint16_t samplePeriod = 0 ;

static signed phaseSamp = 0;

static uint8_t validPhaseSamp = 0;

int gainControl = 1000 ;

uint16_t rpm;

signed count;

signed delta;

void recalcPhase()

{

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

}

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;

}

void processPhase ( void )

{

    // lpcl

        chSysLock();

        const signed pdClip = 10000;

        static signed pd;

        if(validPhaseSamp)

        {

         pd =   phaseSamp - nominal;

     validPhaseSamp = 0;

        if(pd > pdClip)

        pd = pdClip;

        if(pd < -pdClip)

                pd = -pdClip;

   }

     chSysUnlock();

     delta = pd;

     static int sampleAverage = 0;

     static int phaseAverage  = 0;

     const int freqScale = 6;

     const int phaseScale = 50;

     sampleAverage = sampleAverage + (samplePeriod - sampleAverage/freqScale);

     int32_t arr;

   //   if(lock)

       int intSample = sampleAverage / freqScale;

       int deltaPd= pd/ phaseScale;

       if(deltaPd == 0)

       {

       if(pd > 0)

           deltaPd =1;

       if(pd<  0)

           deltaPd =-1;

       }

           arr =  intSample + deltaPd;

// clamp values

    if(arr > 65535)

        arr = 65535;

    if(arr < 1000)

        arr = 1000;

     count = arr;

    TIM2->ARR = arr -1;

    nominal = intSample * (long) (phase10)/ 3600;

    float nomRPM = 30E6 / (MICROSECS_PULSE * arr);

        rpm =  nomRPM ;

        adjustRPM();

    }

// set the timing advance from reference in 0.1 degrees units

void setAdvance(int16_t deg10)

{

    phase10 = deg10;

}

// specialist timer setup :

// timer 2 is a reloading counter with a cycle period controlled by its ARR register.

// Just before terminal count it produces a pulse of 200 microseconds using its CCR1 count compare register,

// used to drive the strobe LED.

// Timer 3 is then used to count the time of the reload of Timer 2 via its Trigger Out being selected as reload.

// The time is latched in TIM3  CCR2

// and ignition pulses are latched on TIM3 CCR1, to allow it to be used in a PLL.

//

void initTimers()

{

        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

    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;

}

// timer 3 interrupt

void VectorB4(void)

{

        if(TIM3->SR & TIM_SR_CC1IF)

        {

                uint16_t sample = TIM3->CCR1;

//              if(sample-lastSampleRef >  1000 /*BREAKER_COUNT_MIN */)

                {

                samplePeriod = sample-sampleRef;

                sampleRef = sample;

                        ++refCount;

                }

                lastSampleRef= sample;

        }

        if(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 < varToRef)

                phaseSamp = refToVar;

        else if(varToRef <= refToVar)

                phaseSamp = -varToRef;

        }

        validPhaseSamp = 1;

        refCount=0;

        varCount=0;

    }

    // frequency error, should deal with by direct period measurement

    if(refCount > 1 || varCount > 1  )

    {

        refCount = 0;

        varCount = 0;

    }

}