WIP

1 files changed, 74 insertions, 33 deletions

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<<TIM_CR2_MMS_Pos); /* Update event on TRGO */
     TIM3->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<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_MINC | DMA_CCR_CIRC | DMA_CCR_HTIE | DMA_CCR_TCIE;
+    DMA1_Channel1->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<<ADC_CFGR1_EXTEN_Pos) | (3<<ADC_CFGR1_EXTSEL_Pos) | ADC_CFGR1_DMAEN | ADC_CFGR1_DMACFG; /* TIM3 TRGO */
-    ADC1->CFGR2 = (1<<ADC_CFGR2_CKMODE_Pos) | (4<<ADC_CFGR2_OVSR_Pos) | (1<<ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_TOVS | ADC_CFGR2_OVSE; /* Oversample by 16 */
+    ADC1->CFGR2 = (1<<ADC_CFGR2_CKMODE_Pos) | (4<<ADC_CFGR2_OVSR_Pos) | (1<<ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_OVSE;
     ADC1->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<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_MINC | DMA_CCR_TCIE;
-        DMA1_Channel1->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<COUNT_OF(adc_data)-1; i++) {
-            uint32_t a = adc_data[i], b = adc_data[i+1];
+        for (ssize_t i=0; i<192-1; i++) {
+            uint32_t a = data[i], b = data[i+1];
 
             if (state == 0) {
                 if (a < lower_thr && b > 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<num_edges-1; i++) {
+                int delta = edge_indices[i+1] - edge_indices[i];
+                if (delta > 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<num_edges; i++) {
+                int remainder = edge_indices[i] - offset;
+                while (remainder > 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; i<keep; i++) {
-        bottom += adc_data[discard + i];
-        top += adc_data[COUNT_OF(adc_data) - 1 - discard - i];
+        bottom += data[discard + i];
+        top += data[COUNT_OF(data)/2 - 1 - discard - i];
     }
 
     bottom /= keep;
     top /= keep;
-    sync_running = false;
+
+    TIM3->ARR = 125*32 + phase_correction; 
+    GPIOB->BRR = (1<<7);
 }
 
 void delay_us(int duration_us) {