From dcfb42c972f904482514ef194003018a02c8c260 Mon Sep 17 00:00:00 2001 From: Trygve Laugstøl Date: Wed, 31 May 2017 23:51:56 +0200 Subject: o Lots of work, IC not really working. Almost there. --- src/radio-controller.cpp | 223 ++++++++++++++++++++++++++++++++--------------- 1 file changed, 154 insertions(+), 69 deletions(-) (limited to 'src') 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 #include #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 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 +#include +#include + +template +struct from_f_cpu_and_time_per_timer_tick { + template + using r = std::ratio; + + using f_cpu = r; + using seconds_per_cpu_tick = r<1, f_cpu::num>; + using seconds_per_timer_tick = SecondsPerTimerTick; + using timer_frequency = r; + +// static constexpr std::intmax_t prescaler = std::ratio_divive::value; + using prescaler_t = std::ratio_divide; + 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>::type; + + static constexpr uint16_t us_per_timer_tick = static_cast(us_per_timer_tick_t::num); + + template + __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::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(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(reinterpret_cast(str)), 5); +// auto *str = "1234567890\n"; +// CDC_Transmit_FS(const_cast(reinterpret_cast(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(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(input_capture)); - buffer_lock.unlock(); +// if (buffer.check_first()) { +// return; +// } + + static uint16_t value1, value2; + + if (sample_index == 0) { +// value1 = static_cast(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1)); + value1 = static_cast(htim->Instance->CCR1); + sample_index = 1; + } else { +// value2 = static_cast(HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1)); + value2 = static_cast(htim->Instance->CCR1); + sample_index = 0; + + uint16_t diff; + if (value2 > value1) { + diff = value2 - value1; + } else if (value1 > value2) { + diff = (static_cast(0xffff) - value1) + value2 + static_cast(1); + } else { + return; + } + + if (buffer_lock.try_lock()) { + buffer.append({values::to_us(0), values::to_us(diff)}); + + buffer_lock.unlock(); + } } } -- cgit v1.2.3