o Lots of work, IC not really working. Almost there.

1 files changed, 154 insertions, 69 deletions

diff --git a/src/radio-controller.cpp b/src/radio-controller.cpp
index 61cacfd..0f60717 100644
--- a/src/radio-controller.cpp
+++ b/src/radio-controller.cpp
@@ -1,80 +1,63 @@
 #include <usbd_cdc_if.h>
 #include <cinttypes>
 #include "radio-controller.h"
+#include "mcu/arm/mutex.h"
 #include "mcu/arm/semihosting.h"
 #include "mcu/stm32cube/uart.h"
 #include "mcu/stm32cube/debug.h"
 
-#ifdef HAL_IWDG_MODULE_ENABLED
-extern IWDG_HandleTypeDef hiwdg;
-#endif
+using mcu::arm::mutex;
 
+extern IWDG_HandleTypeDef hiwdg;
 extern TIM_HandleTypeDef htim1;
 extern UART_HandleTypeDef huart2;
 
 mcu::stm32cube::uart::uart_port uart2(&huart2);
 mcu::stm32cube::debug::dbg<100> dbg(uart2);
 
-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;
-    }
+struct sample {
+    uint16_t period_us;
+    uint16_t pulse_us;
 };
 
 template<unsigned int BufferSize>
 class sample_buffer {
-    interval_us samples[BufferSize];
+    sample samples[BufferSize];
 
     volatile unsigned int _size;
 
+    bool _first;
+
 public:
     void reset()
     {
         _size = 0;
+        _first = true;
     }
 
-    unsigned int size()
+    bool check_first()
+    {
+        if (_first) {
+            _first = false;
+            return true;
+        }
+        return false;
+    }
+
+    __always_inline
+    unsigned int size() const
     {
         return _size;
     }
 
-    bool is_full()
+    __always_inline
+    bool is_full() const
     {
         return _size == BufferSize;
     }
 
-    __noinline
-    void append(interval_us sample)
+    __always_inline
+    void append(sample sample)
     {
         if (_size == BufferSize) {
             return;
@@ -83,12 +66,14 @@ public:
         samples[_size++] = sample;
     }
 
-    interval_us at(int i)
+    __always_inline
+    sample at(int i) const
     {
         return samples[i];
     }
 };
 
+volatile int sample_index;
 sample_buffer<10> buffer;
 mutex buffer_lock;
 
@@ -96,6 +81,15 @@ void main_pre_init()
 {
 }
 
+inline void hal_ok(HAL_StatusTypeDef status)
+{
+    if (__predict_true(status == HAL_OK)) {
+        return;
+    }
+
+    halt();
+}
+
 void main_post_init()
 {
     bool debugger_connected = (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) != 0;
@@ -105,64 +99,155 @@ void main_post_init()
 
     printf("Radio Controller\n");
 
-//    HAL_TIM_Base_Start_IT(&htim1);
-    if (HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1) != HAL_OK) {
-        halt();
-    }
-
+    buffer.reset();
+    sample_index = 0;
     uart2.enable();
 
-    buffer.reset();
+    hal_ok(HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1));
 }
 
 static uint32_t tick_next = 0;
-static uint32_t cnt_prev = 0;
 
-static bool seen_high = false;
+//static bool seen_high = false;
+
+#include <chrono>
+#include <stm32f1xx_hal_tim.h>
+#include <stm32f103x6.h>
+
+template<unsigned Fcpu, typename SecondsPerTimerTick>
+struct from_f_cpu_and_time_per_timer_tick {
+    template<std::intmax_t Num, std::intmax_t Den>
+    using r = std::ratio<Num, Den>;
+
+    using f_cpu = r<Fcpu, 1>;
+    using seconds_per_cpu_tick = r<1, f_cpu::num>;
+    using seconds_per_timer_tick = SecondsPerTimerTick;
+    using timer_frequency = r<SecondsPerTimerTick::den, SecondsPerTimerTick::num>;
+
+//    static constexpr std::intmax_t prescaler = std::ratio_divive<f_cpu, timer_frequency>::value;
+    using prescaler_t = std::ratio_divide<f_cpu, timer_frequency>;
+    static_assert(prescaler_t::den == 1, "Could not represent the prescaler as an integer");
+    static constexpr std::intmax_t prescaler = prescaler_t::num;
+
+    // us_per_timer_tick = seconds_per_timer_tick * 1'000'000
+    using us_per_timer_tick_t = typename std::ratio_multiply<seconds_per_timer_tick, r<1'000'000, 1>>::type;
+
+    static constexpr uint16_t us_per_timer_tick = static_cast<uint16_t>(us_per_timer_tick_t::num);
+
+    template<typename T>
+    __always_inline constexpr
+    static T to_us(unsigned int count)
+    {
+        return T(count * us_per_timer_tick);
+    }
+};
+
+using values = from_f_cpu_and_time_per_timer_tick<72'000'000, std::ratio_multiply<std::ratio<5, 1>, std::micro>>;
+
+//static_assert(values::timer_frequency::num == 200'000, "timer_frequency::num");
+static_assert(values::timer_frequency::den == 1, "timer_frequency::den");
+//static_assert(std::is_same<
+//    values::timer_frequency,
+//    std::ratio<200'000, 1>>::value, "Timer frequency is not 200kHz");
+static_assert(values::prescaler == 360, "Timer frequency is not 200kHz");
+static_assert(values::prescaler - 1 == RADIO_RX_TIMER_PRESCALER, "Bad prescaler from STM32CubeMX");
+static_assert(values::us_per_timer_tick == 5, "bad us_per_timer_tick");
 
 void main_loop()
 {
     auto now = HAL_GetTick();
 
     if (now >= tick_next) {
-        uint32_t cnt = __HAL_TIM_GET_COUNTER(&htim1);
-        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;
+        printf("now=%" PRIu32 "\n", now);
 
-        auto *str = "1234567890\n";
-        CDC_Transmit_FS(const_cast<uint8_t *>(reinterpret_cast<const uint8_t *>(str)), 5);
+//        auto *str = "1234567890\n";
+//        CDC_Transmit_FS(const_cast<uint8_t *>(reinterpret_cast<const uint8_t *>(str)), 5);
 
         tick_next += 1000;
-        seen_high = false;
+//        seen_high = false;
     }
 
     if (buffer.is_full()) {
         buffer_lock.lock();
 
+        hal_ok(HAL_TIM_IC_Stop_IT(&htim1, TIM_CHANNEL_1));
+
+//            printf("Period:");
+//            for (unsigned int i = 0; i < buffer.size(); i++) {
+//                printf(" %u", buffer.at(i));
+//            }
+//            printf("\n");
+
+//            dbg.print("Pulse: ");
+//            for (unsigned int i = 0; i < buffer.size(); i++) {
+//                auto p = values::to_us(buffer_pulse.at(i));
+//                dbg.print(" %05u", p);
+//            }
+//            dbg.print("\n");
+//
+//            dbg.print("Period:");
+//            for (unsigned int i = 0; i < buffer.size(); i++) {
+//                auto p = values::to_us(buffer_period.at(i));
+//                dbg.print(" %05u", p);
+//            }
+//            dbg.print("\n");
+
+        dbg.println("Samples");
         for (unsigned int i = 0; i < buffer.size(); i++) {
-            dbg.println("%d", static_cast<int>(buffer.at(i)));
+            sample s = buffer.at(i);
+            auto pulse = s.pulse_us;
+            auto period = s.period_us;
+            dbg.println("%d %d, %d%%", period, pulse, int(pulse / double(period) * 100));
         }
-        dbg.println("\n");
+        dbg.println();
 
         buffer.reset();
-//        buffer_lock.unlock();
+        sample_index = 0;
+        hal_ok(HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1));
+        buffer_lock.unlock();
     }
 
-    seen_high |= HAL_GPIO_ReadPin(RADIO_RX_GPIO_Port, RADIO_RX_Pin);
+//    seen_high |= HAL_GPIO_ReadPin(RADIO_RX_GPIO_Port, RADIO_RX_Pin);
 
-#ifdef HAL_IWDG_MODULE_ENABLED
     HAL_IWDG_Refresh(&hiwdg);
-#endif
 }
 
+//static uint16_t start = 0;
+
 void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
 {
-    auto input_capture = __HAL_TIM_GET_COMPARE(htim, TIM_CHANNEL_1);
+    if (htim->Channel != HAL_TIM_ACTIVE_CHANNEL_1) {
+        return;
+    }
 
-    if (buffer_lock.try_lock()) {
-        buffer.append(static_cast<uint16_t>(input_capture));
-        buffer_lock.unlock();
+//    if (buffer.check_first()) {
+//        return;
+//    }
+
+    static uint16_t value1, value2;
+
+    if (sample_index == 0) {
+//        value1 = static_cast<uint16_t>(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1));
+        value1 = static_cast<uint16_t>(htim->Instance->CCR1);
+        sample_index = 1;
+    } else {
+//        value2 = static_cast<uint16_t>(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1));
+        value2 = static_cast<uint16_t>(htim->Instance->CCR1);
+        sample_index = 0;
+
+        uint16_t diff;
+        if (value2 > value1) {
+            diff = value2 - value1;
+        } else if (value1 > value2) {
+            diff = (static_cast<uint16_t>(0xffff) - value1) + value2 + static_cast<uint16_t>(1);
+        } else {
+            return;
+        }
+
+        if (buffer_lock.try_lock()) {
+            buffer.append({values::to_us<uint16_t>(0), values::to_us<uint16_t>(diff)});
+
+            buffer_lock.unlock();
+        }
     }
 }