From f87404cba6a49acb90c82ca2d1bf010d0dc8445f Mon Sep 17 00:00:00 2001
From: chauyin <chauyin@chauyin.com>
Date: Sat, 10 May 2025 18:23:21 +0800
Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=20Ultrasonic=20=E7=B1=BB?=
 =?UTF-8?q?=E5=92=8C=20TaskHelper=20=E7=B1=BB=EF=BC=8C=E6=8F=90=E4=BE=9B?=
 =?UTF-8?q?=E8=B6=85=E5=A3=B0=E6=B3=A2=E6=B5=8B=E8=B7=9D=E5=8A=9F=E8=83=BD?=
 =?UTF-8?q?=E5=8F=8A=E4=BB=BB=E5=8A=A1=E7=AD=89=E5=BE=85=E8=BE=85=E5=8A=A9?=
 =?UTF-8?q?=E6=96=B9=E6=B3=95?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Core/App/Common/ultrasonic.hpp  | 89 +++++++++++++++++++++++++++++++++
 Core/App/Helper/task_helper.hpp | 19 +++++++
 2 files changed, 108 insertions(+)
 create mode 100644 Core/App/Common/ultrasonic.hpp
 create mode 100644 Core/App/Helper/task_helper.hpp

diff --git a/Core/App/Common/ultrasonic.hpp b/Core/App/Common/ultrasonic.hpp
new file mode 100644
index 0000000..3ff3af9
--- /dev/null
+++ b/Core/App/Common/ultrasonic.hpp
@@ -0,0 +1,89 @@
+#pragma once
+
+#include "../Helper/delay_helper.h"
+#include "stm32h563xx.h"
+#include "stm32h5xx_hal_tim.h"
+#include <cstdint>
+#include <stdexcept>
+
+#include "../Helper/gpio_helper.hpp"
+#include "../Helper/task_helper.hpp"
+
+class Ultrasonic {
+public:
+    Ultrasonic(GPIO_TypeDef* gpio, uint16_t pin, TIM_HandleTypeDef* timer, uint32_t channel)
+        : Ultrasonic({gpio, pin}, timer, channel) {}
+
+    Ultrasonic(const GpioHelper::Gpio& trigger, TIM_HandleTypeDef* timer, uint32_t channel)
+        : trigger(trigger), timer(timer), channel(channel) {
+        captureFlag = GetCaptureFlag(channel);
+    }
+
+    double GetDistance(uint32_t time = 1) {
+        double distance = 0;
+        for (uint32_t cnt = 0; cnt < time; ++cnt) {
+            Trigger();
+            auto echoTime = MeasureEchoTime();
+            distance += CalculateDistance(echoTime);
+        }
+        return distance / time;
+    }
+
+private:
+    constexpr static uint32_t kTimeout = 100;
+
+    GpioHelper::Gpio trigger;
+    TIM_HandleTypeDef* timer;
+    uint32_t channel;
+    uint32_t captureFlag;
+
+    void Trigger() {
+        trigger.Set();
+        DelayUs(10);
+        trigger.Reset();
+    }
+
+    double MeasureEchoTime() {
+        timer->Instance->CNT = 0;
+        uint32_t data[2];
+        HAL_TIM_IC_Start(timer, channel);
+        try {
+            for (auto& value : data) {
+                TaskHelper::WaitFor(
+                    [&]() {
+                        if (__HAL_TIM_GET_FLAG(timer, captureFlag)) {
+                            __HAL_TIM_CLEAR_FLAG(timer, captureFlag);
+                            value = HAL_TIM_ReadCapturedValue(timer, channel);
+                            return true;
+                        }
+                        return false;
+                    },
+                    kTimeout);
+            }
+            HAL_TIM_IC_Stop(timer, channel);
+            return (data[1] - data[0]) / 250.0;
+        } catch (const std::exception& e) {
+            HAL_TIM_IC_Stop(timer, channel);
+            throw;
+        }
+    }
+
+    [[nodiscard]] static double CalculateDistance(double echoTime) {
+        return (echoTime * 0.0343) / 2.0;
+    }
+
+    static uint32_t GetCaptureFlag(uint32_t channel) {
+        switch (channel) {
+        case TIM_CHANNEL_1:
+            return TIM_SR_CC1IF;
+        case TIM_CHANNEL_2:
+            return TIM_SR_CC2IF;
+        case TIM_CHANNEL_3:
+            return TIM_SR_CC3IF;
+        case TIM_CHANNEL_4:
+            return TIM_SR_CC4IF;
+        default:
+            throw std::invalid_argument("Invalid timer channel");
+        }
+    }
+};
diff --git a/Core/App/Helper/task_helper.hpp b/Core/App/Helper/task_helper.hpp
new file mode 100644
index 0000000..5780067
--- /dev/null
+++ b/Core/App/Helper/task_helper.hpp
@@ -0,0 +1,19 @@
+#pragma once
+
+#include "stm32h5xx_hal.h"
+#include <cstdint>
+#include <stdexcept>
+
+class TaskHelper {
+public:
+    template <typename Func>
+    static void WaitFor(Func func, int timeoutMilliseconds) {
+        auto start = HAL_GetTick();
+        while (!func()) {
+            uint32_t now = HAL_GetTick();
+            if ((now - start) > static_cast<uint32_t>(timeoutMilliseconds)) {
+                throw std::runtime_error("Operation timed out");
+            }
+        }
+    }
+};
\ No newline at end of file