void TIM1_init(void) {
SIM->SCGC5 |= SIM_SCGC5_PORTB(1);
PORTB->PCR[0] = PORT_PCR_MUX(0x3); // Set pin as ALT3 (TIM1_CH1)
SIM->SCGC6 |= SIM_SCGC6_FTM1(1);
FTM1->SC = 0;
FTM1->MOD = 1999;
FTM1->CONTROLS[1].CnSC = FTM_CnSC_MSB_MASK | FTM_CnSC_ELSB_MASK;
FTM1->CONTROLS[1].CnV = 1000;
FTM1->SC = FTM_SC_PS(7) | FTM_SC_TOF(1);
}
void TIM2_init(void) {
// Enable clock for PORTA
SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK;
PORTA->PCR[1] = PORT_PCR_MUX(0x3); // Set pin as ALT3 (TIM2_CH1)
SIM->SCGC6 |= SIM_SCGC6_FTM2(1);
FTM2->SC = 0;
FTM2->CONTROLS[1].CnSC = FTM_CnSC_ELSA_MASK | FTM_CnSC_ELSB_MASK |FTM_CnSC_CHIE_MASK;
FTM2->SC = FTM_SC_PS(7) | FTM_SC_TOF(1);
NVIC_EnableIRQ(FTM2_IRQn);
}
void FTM2_IRQHandler() {
// Clear channel flag
FTM2->CONTROLS[1].CnSC &= ~FTM_CnSC_CHF_MASK;
if (gap == 0) {
counter0 = FTM2->CONTROLS[1].CnV;
gap = 1;
} else {
counter1 = FTM2->CONTROLS[1].CnV;
if (counter1 > counter0) {
Counter = counter1 - counter0;
Frequency = 10000 / Counter;
} else {
Counter = counter1 + 0xFFFF - counter0 + 1;
Frequency = 10000 / Counter;
}
counter0 = counter1;
}
}