1 files changed, 80 insertions, 2 deletions

diff --git a/center_fw/src/main.c b/center_fw/src/main.c
index a521acb..0983333 100644
--- a/center_fw/src/main.c
+++ b/center_fw/src/main.c
@@ -63,7 +63,7 @@ int main(void) {
 
     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 16 times oversampling. */
+    /* 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;
     
@@ -100,6 +100,7 @@ int main(void) {
     GPIOC->MODER = OUT(15) | ANALOG(14) | ANALOG(9);
 
     DBG->APBFZ1 |= DBG_APB_FZ1_DBG_TIM3_STOP;
+    DBG->APBFZ2 |= DBG_APB_FZ2_DBG_TIM1_STOP;
     while (42) {
     }
 }
@@ -124,9 +125,86 @@ void TIM1_CC_IRQHandler(void) {
 }
 
 void DMA1_Channel1_IRQHandler(void) {
+    static int32_t bottom = -1;
+    static int32_t top = -1;
+
     DMA1->IFCR = DMA_IFCR_CTCIF1;
+
+    if (bottom >= 0) {
+        uint32_t amplitude = top - bottom;
+        uint32_t middle = bottom + amplitude / 2;
+
+        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 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];
+
+            if (state == 0) {
+                if (a < lower_thr && b > lower_thr) {
+                    state = 1;
+                    run_start = i+1;
+                } else if (a > upper_thr && b < upper_thr) {
+                    state = -1;
+                    run_start = i+1;
+                }
+
+            } else if (state == 1) {
+                if (b < a) {
+                    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];
+                    int d = a - v0;
+                    int c = i - run_start;
+                    size_t intercept = run_start * adc_clockdiv + (middle - v0) * adc_clockdiv * c / d;
+                    if (num_edges < COUNT_OF(edge_indices)) {
+                        edge_indices[num_edges] = intercept;
+                        num_edges++;
+                    }
+                    state = 0;
+                }
+
+            } else if (state == -1) {
+                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];
+                    int d = a - v0;
+                    int c = i - run_start;
+                    size_t intercept = run_start * adc_clockdiv + (middle - v0) * adc_clockdiv * c / d;
+                    if (num_edges < COUNT_OF(edge_indices)) {
+                        edge_indices[num_edges] = intercept;
+                        num_edges++;
+                    }
+                    state = 0;
+                }
+            }
+        }
+
+        asm volatile ("bkpt");
+    }
+
+    const int discard = 5;
+    const int keep = 32;
+
     quicksort(adc_data, &adc_data[COUNT_OF(adc_data)-1]);
-    asm volatile ("bkpt");
+    for (size_t i=0; i<keep; i++) {
+        bottom += adc_data[discard + i];
+        top += adc_data[COUNT_OF(adc_data) - 1 - discard - i];
+    }
+
+    bottom /= keep;
+    top /= keep;
     sync_running = false;
 }