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 4

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

static uint8_t locked = 0; // in lock if true

int gainControl = 1000;

uint16_t rpm;

signed count;

signed delta;

void

recalcPhase ()

{

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

}

void

adjustRPM (void)

{

  if (rpm < 500)

    rpm = 500;

  if (rpm > 10000)

    rpm = 10000;

}

uint16_t

setRPM (uint16_t rpm_)

{

  if (rpm_ >= 500 && rpm_ < 10000)

    {

      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

  const signed pdClip = 1000;

  static signed pd;

  if (validPhaseSamp)

    {

      chSysLock ();

      pd = phaseSamp - nominal;

      validPhaseSamp = 0;

      chSysUnlock ();

      if (pd > pdClip)

        pd = pdClip;

      if (pd < -pdClip)

        pd = -pdClip;

    }

  else

    return;

  delta = pd;

  static int sampleAverage = 0;

  static int phaseAverage = 0;

  const int freqScale = 1024;  // scale up calculations

  const int periodScale = 8; // 1/periodScale  of difference between period measured and average is added to period.

  const int phaseScale = 500;

  const int phaseLim = 1; // total contribution of phase accumulator

  // measure sample period devi

  sampleAverage = sampleAverage

      + (samplePeriod * freqScale - sampleAverage) / periodScale;

  int32_t arr;

  //    if(lock)

  int intSample = sampleAverage;

  // dont phase lock until r and v in lock

  if (locked == 1)

    {

      phaseAverage += pd;

    }

  else

    {

      phaseAverage = 0;

    }

  if(phaseAverage > phaseScale * phaseLim)

    phaseAverage = phaseScale * phaseLim;

  if (phaseAverage < -phaseScale * phaseLim)

    phaseAverage = -phaseScale *phaseLim;

  arr = sampleAverage / freqScale + phaseAverage / phaseScale;

// clamp values

  if (arr > 65535)

    arr = 65535;

  if (arr < 1000)

    arr = 1000;

  count = arr;

  TIM2->ARR = arr - 1;

  nominal = count * (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;

      locked = 1;

    }

  if (refCount > 1 || varCount > 1)

    {

      refCount = 0;

      varCount = 0;

      locked = 0;

    }

}