/*
* 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];
volatile uint8_t haveSlowPulse = 0;
uint16_t rpm;
uint16_t count;
uint16_t 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;
}
unsigned getDelta(void)
{
return delta;
}
unsigned getCount(void)
{
return count;
}
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/ 2000;
// phase detector returns +/
float const wn = 0.01f;
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 ;
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)
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;
}
}