1 files changed, 121 insertions, 13 deletions

diff --git a/src/radio-controller.cpp b/src/radio-controller.cpp
index 461a910..61cacfd 100644
--- a/src/radio-controller.cpp
+++ b/src/radio-controller.cpp
@@ -1,5 +1,6 @@
+#include <usbd_cdc_if.h>
+#include <cinttypes>
 #include "radio-controller.h"
-#include "stm32f1xx_hal.h"
 #include "mcu/arm/semihosting.h"
 #include "mcu/stm32cube/uart.h"
 #include "mcu/stm32cube/debug.h"
@@ -14,10 +15,89 @@ extern UART_HandleTypeDef huart2;
 mcu::stm32cube::uart::uart_port uart2(&huart2);
 mcu::stm32cube::debug::dbg<100> dbg(uart2);
 
-void main_pre_init() {
+using interval_us = uint16_t;
+
+class mutex {
+    volatile uint8_t locked;
+
+public:
+    mutex() : locked(0)
+    {}
+
+    __always_inline
+    bool try_lock()
+    {
+        auto l = __LDREXB(&locked);
+
+        if (l == 1) {
+            return false;
+        }
+
+        return __STREXB(1, &locked) == 0;
+    }
+
+    __always_inline
+    bool lock()
+    {
+        do {
+            while(__LDREXB(&locked) == 1);
+        } while(__STREXB(1, &locked) != 0);
+    }
+
+    __always_inline
+    bool unlock()
+    {
+        locked = 0;
+    }
+};
+
+template<unsigned int BufferSize>
+class sample_buffer {
+    interval_us samples[BufferSize];
+
+    volatile unsigned int _size;
+
+public:
+    void reset()
+    {
+        _size = 0;
+    }
+
+    unsigned int size()
+    {
+        return _size;
+    }
+
+    bool is_full()
+    {
+        return _size == BufferSize;
+    }
+
+    __noinline
+    void append(interval_us sample)
+    {
+        if (_size == BufferSize) {
+            return;
+        }
+
+        samples[_size++] = sample;
+    }
+
+    interval_us at(int i)
+    {
+        return samples[i];
+    }
+};
+
+sample_buffer<10> buffer;
+mutex buffer_lock;
+
+void main_pre_init()
+{
 }
 
-void main_post_init() {
+void main_post_init()
+{
     bool debugger_connected = (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) != 0;
     if (debugger_connected) {
         semihosting::enable();
@@ -25,36 +105,64 @@ void main_post_init() {
 
     printf("Radio Controller\n");
 
-    HAL_TIM_Base_Start_IT(&htim1);
-//    HAL_TIM_IC_Start_IT(&htim1, 1);
+//    HAL_TIM_Base_Start_IT(&htim1);
+    if (HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1) != HAL_OK) {
+        halt();
+    }
 
     uart2.enable();
+
+    buffer.reset();
 }
 
 static uint32_t tick_next = 0;
+static uint32_t cnt_prev = 0;
 
 static bool seen_high = false;
 
-void main_loop() {
+void main_loop()
+{
     auto now = HAL_GetTick();
 
     if (now >= tick_next) {
-//        puts(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_8) ? "1" : "0");
-
         uint32_t cnt = __HAL_TIM_GET_COUNTER(&htim1);
-        dbg.println("cnt=%lu, now=%lu, seen_high=%d", cnt, now, seen_high);
+        auto diff = static_cast<int>(cnt - cnt_prev);
+        dbg.println("cnt=%lu, diff=%d, now=%" PRIu32 ", seen_high=%d, buffer_size=%d", cnt, diff, now, seen_high,
+                    buffer.size());
+        cnt_prev = cnt;
+
+        auto *str = "1234567890\n";
+        CDC_Transmit_FS(const_cast<uint8_t *>(reinterpret_cast<const uint8_t *>(str)), 5);
+
         tick_next += 1000;
         seen_high = false;
-        dbg.println("wat");
     }
 
-    seen_high |= HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_8);
+    if (buffer.is_full()) {
+        buffer_lock.lock();
+
+        for (unsigned int i = 0; i < buffer.size(); i++) {
+            dbg.println("%d", static_cast<int>(buffer.at(i)));
+        }
+        dbg.println("\n");
+
+        buffer.reset();
+//        buffer_lock.unlock();
+    }
+
+    seen_high |= HAL_GPIO_ReadPin(RADIO_RX_GPIO_Port, RADIO_RX_Pin);
 
 #ifdef HAL_IWDG_MODULE_ENABLED
     HAL_IWDG_Refresh(&hiwdg);
 #endif
 }
 
-void it_tim1() {
-    ;
+void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
+{
+    auto input_capture = __HAL_TIM_GET_COMPARE(htim, TIM_CHANNEL_1);
+
+    if (buffer_lock.try_lock()) {
+        buffer.append(static_cast<uint16_t>(input_capture));
+        buffer_lock.unlock();
+    }
 }