From 85e6e19af9e97351f408bacc01fdee92f4e19903 Mon Sep 17 00:00:00 2001 From: jaseg Date: Thu, 28 Sep 2023 22:28:44 +0200 Subject: WIP --- center_fw/src/main.c | 107 +++++++++++++++++++++++++++++++++++---------------- 1 file changed, 74 insertions(+), 33 deletions(-) (limited to 'center_fw/src') diff --git a/center_fw/src/main.c b/center_fw/src/main.c index 0983333..4df1db5 100644 --- a/center_fw/src/main.c +++ b/center_fw/src/main.c @@ -17,8 +17,9 @@ #include "global.h" -uint16_t adc_data[192]; -bool sync_running = false; +uint16_t adc_data[192*2]; + +const int nominal_period = 125*16*32/2; static void quicksort(uint16_t *head, uint16_t *tail); @@ -51,8 +52,9 @@ int main(void) { RCC->APBENR2 |= RCC_APBENR2_TIM1EN | RCC_APBENR2_ADCEN; RCC->IOPENR |= RCC_IOPENR_GPIOAEN | RCC_IOPENR_GPIOBEN | RCC_IOPENR_GPIOCEN; - TIM1->PSC = 1; - TIM1->ARR = 32767; + /* + TIM1->PSC = 0; + TIM1->ARR = nominal_period; TIM1->DIER = TIM_DIER_UIE | TIM_DIER_CC1IE; TIM1->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN; TIM1->CCR1 = 3000; @@ -60,16 +62,21 @@ int main(void) { NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 0); NVIC_EnableIRQ(TIM1_CC_IRQn); NVIC_SetPriority(TIM1_CC_IRQn, 0); + */ + TIM3->CR1 = TIM_CR1_ARPE; TIM3->CR2 = (2<PSC = 0; /* We sample 32 times per 1 kHz AC cycle, and use 32 times oversampling. */ - TIM3->ARR = 124; /* Output 64 MHz / 125 = 512 kHz signal */ - TIM3->CR1 = TIM_CR1_CEN; + TIM3->ARR = 125*32; /* Output 64 MHz / 125 = 512 kHz signal */ + TIM3->CR1 |= TIM_CR1_CEN; DMAMUX1[0].CCR = 5; /* ADC */ DMA1_Channel1->CPAR = (uint32_t)&ADC1->DR; DMA1_Channel1->CMAR = (uint32_t)(void *)adc_data; + DMA1_Channel1->CNDTR = COUNT_OF(adc_data); + DMA1_Channel1->CCR = (1<CCR |= DMA_CCR_EN; NVIC_EnableIRQ(DMA1_Channel1_IRQn); NVIC_SetPriority(DMA1_Channel1_IRQn, 64); @@ -82,7 +89,7 @@ int main(void) { /* wait. */ } ADC1->CFGR1 = (1<CFGR2 = (1<CFGR2 = (1<CHSELR = (1<<4); /* Enable input 4 -> PA4 (Vdiff)*/ while (!(ADC1->ISR & ADC_ISR_CCRDY)) { /* wait. */ @@ -105,30 +112,23 @@ int main(void) { } } +/* void TIM1_BRK_UP_TRG_COM_IRQHandler(void) { TIM1->SR &= ~TIM_SR_UIF; - GPIOB->BSRR = (1<<7); - if (!sync_running) { - while(DMA1_Channel1->CCR != 0) { - DMA1_Channel1->CCR = 0; - } - DMA1_Channel1->CCR = (1<CNDTR = COUNT_OF(adc_data); - DMA1_Channel1->CCR |= DMA_CCR_EN; - sync_running = true; - } } void TIM1_CC_IRQHandler(void) { TIM1->SR &= ~TIM_SR_CC1IF; - GPIOB->BRR = (1<<7); } +*/ void DMA1_Channel1_IRQHandler(void) { static int32_t bottom = -1; static int32_t top = -1; - - DMA1->IFCR = DMA_IFCR_CTCIF1; + DMA1->IFCR = DMA_IFCR_CGIF1; + int phase_correction = 0; + GPIOB->BSRR = (1<<7); + uint16_t *data = DMA1->ISR & DMA_ISR_HTIF1 ? &adc_data[0] : &adc_data[192]; if (bottom >= 0) { uint32_t amplitude = top - bottom; @@ -137,16 +137,15 @@ void DMA1_Channel1_IRQHandler(void) { const uint32_t lower_thr = bottom + amplitude / 4; const uint32_t upper_thr = top - amplitude / 4; const uint32_t adc_clockdiv = 125 * 32; - //const uint32_t adc_clockdiv = 1; /* FIXME DEBUG */ - size_t num_edges = 0; - ssize_t edge_indices[24]; + int num_edges = 0; + int edge_indices[24]; int state = 0; ssize_t run_start = -1; int last_v = -1; - for (ssize_t i=0; i lower_thr) { @@ -162,7 +161,7 @@ void DMA1_Channel1_IRQHandler(void) { state = 0; } else if (a < upper_thr && b > upper_thr) { /* run from run_start (incl.) to i (incl.) */ - uint32_t v0 = adc_data[run_start]; + uint32_t v0 = data[run_start]; int d = a - v0; int c = i - run_start; size_t intercept = run_start * adc_clockdiv + (middle - v0) * adc_clockdiv * c / d; @@ -177,8 +176,7 @@ void DMA1_Channel1_IRQHandler(void) { if (b > a) { state = 0; } else if (a > lower_thr && b < lower_thr) { - /* run from run_start (incl.) to i (incl.) */ - uint32_t v0 = adc_data[run_start]; + uint32_t v0 = data[run_start]; int d = a - v0; int c = i - run_start; size_t intercept = run_start * adc_clockdiv + (middle - v0) * adc_clockdiv * c / d; @@ -191,21 +189,64 @@ void DMA1_Channel1_IRQHandler(void) { } } - asm volatile ("bkpt"); + if (num_edges > 1) { + const int approx_cycle = 32*16*125/2; + int delta_avg = 0; + for (size_t i=0; i approx_cycle/2 * 50) { + asm volatile ("bkpt"); + } + while (delta > approx_cycle/2) { + delta -= approx_cycle; + } + delta_avg += delta; + } + delta_avg /= num_edges-1; + + int cycle = approx_cycle + delta_avg; + int offset = 0; + int phase_avg = 0; + for (size_t i=0; i cycle/2) { + remainder -= cycle; + offset += cycle; + } + phase_avg += remainder; + } + phase_avg /= num_edges; + phase_correction = phase_avg >= 0 ? cycle/2 - phase_avg : cycle/2 + phase_avg; + phase_correction /= 125 * 4; + + const int deadzone = 3; + const int max_correction = 40; + if (phase_correction < -deadzone || phase_correction > deadzone) { + if (phase_correction < -max_correction) { + phase_correction = -max_correction; + } else if (phase_correction > max_correction) { + phase_correction = max_correction; + } + } + } } const int discard = 5; const int keep = 32; - quicksort(adc_data, &adc_data[COUNT_OF(adc_data)-1]); + bottom = 0; + top = 0; + quicksort(data, &data[192-1]); for (size_t i=0; iARR = 125*32 + phase_correction; + GPIOB->BRR = (1<<7); } void delay_us(int duration_us) { -- cgit