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