diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
new file mode 100644
index 0000000..c1cd804
--- /dev/null
+++ b/.github/workflows/tests.yml
@@ -0,0 +1,24 @@
+name: tests
+on:
+  push:
+    branches:
+    - main
+  pull_request:
+    branches:
+    - '*'
+jobs:
+  tests:
+    runs-on: ubuntu-22.04
+    timeout-minutes: 10
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          submodules: recursive
+          fetch-depth: 0
+
+      - name: Install gtest
+        run: |
+          wget https://gist.githubusercontent.com/Ponomarevda/d970c24de8deab5d6ccfee8f5f719bcc/raw/install_gtest_ubuntu.sh && chmod +x install_gtest_ubuntu.sh && ./install_gtest_ubuntu.sh
+
+      - name: Build and run tests
+        run: make tests
diff --git a/Makefile b/Makefile
index 321a3e0..b40d0cc 100644
--- a/Makefile
+++ b/Makefile
@@ -55,6 +55,10 @@ checks:
 code_style:
 	cpplint Src/modules/*/*pp Src/peripheral/*/*pp Src/platform/*/*pp
 
+tests:
+	mkdir -p ${BUILD_DIR}/tests
+	cd ${BUILD_DIR}/tests && cmake ../../Src/drivers/as5600/tests && make && ./test_as5600
+
 upload:
 	LATEST_TARGET=$$(ls -td ${BUILD_DIR}/release/*.bin | head -1) && ./scripts/tools/stm32/flash.sh $$LATEST_TARGET
 run:
diff --git a/Src/drivers/as5600/as5600.cpp b/Src/drivers/as5600/as5600.cpp
index f79a743..e731f14 100644
--- a/Src/drivers/as5600/as5600.cpp
+++ b/Src/drivers/as5600/as5600.cpp
@@ -7,6 +7,7 @@
 
 #include "as5600.hpp"
 #include <array>
+#include <algorithm>
 #include "peripheral/i2c/i2c.hpp"
 
 namespace Driver {
@@ -56,17 +57,12 @@ int16_t AS5600::get_angle() {
 
 int16_t AS5600::get_status() {
     auto reg_value = HAL::I2C::read_register_1_byte(I2C_AS5600, REG_STATUS);
-    return (reg_value >= 0) ? ((uint8_t)reg_value & 0b111000) : -reg_value;
+    return (reg_value >= 0) ? ((uint8_t)reg_value & 0b111000) : reg_value;
 }
 
 int16_t AS5600::get_magnitude() {
     auto reg_value = HAL::I2C::read_register_2_bytes(I2C_AS5600, REG_MAGNITUDE);
-
-    if (reg_value < 0) {
-        return reg_value;
-    }
-    int16_t angle_deg = reg_value / 4095.0f * 360.0f;
-    return std::clamp(angle_deg, (int16_t)0, (int16_t)359);
+    return (reg_value >= 0) ? std::clamp(reg_value, (int32_t)0, (int32_t)4095) : reg_value;
 }
 
 }  // namespace Driver
diff --git a/Src/drivers/as5600/as5600.hpp b/Src/drivers/as5600/as5600.hpp
index 1829335..b0c7345 100644
--- a/Src/drivers/as5600/as5600.hpp
+++ b/Src/drivers/as5600/as5600.hpp
@@ -32,7 +32,7 @@ class AS5600 {
 
     /**
      * @brief The MAGNITUDE register indicates the magnitude value of the internal CORDIC.
-     * @return Angle within [0, 360] on success, negative error code otherwise
+     * @return Unitless magnitude within [0, 4095] on success, negative error code otherwise
      */
     int16_t get_magnitude();
 
diff --git a/Src/drivers/as5600/tests/CMakeLists.txt b/Src/drivers/as5600/tests/CMakeLists.txt
new file mode 100644
index 0000000..0870f42
--- /dev/null
+++ b/Src/drivers/as5600/tests/CMakeLists.txt
@@ -0,0 +1,28 @@
+# Copyright (C) 2024 Dmitry Ponomarev <ponomarevda96@gmail.com>
+# Distributed under the terms of the GPL v3 license, available in the file LICENSE.
+
+cmake_minimum_required(VERSION 3.10)
+project(as5600)
+
+add_library(as5600
+    ../as5600.cpp
+    ../../Src/platform/ubuntu/i2c.cpp
+)
+
+target_include_directories(as5600 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+find_package(GTest REQUIRED)
+include_directories(
+    ${GTEST_INCLUDE_DIRS}
+    ../../../../Src
+)
+
+add_executable(test_as5600
+    test_as5600.cpp
+)
+
+target_link_libraries(test_as5600
+    as5600
+    GTest::GTest
+    GTest::Main
+)
diff --git a/Src/drivers/as5600/tests/test_as5600.cpp b/Src/drivers/as5600/tests/test_as5600.cpp
new file mode 100644
index 0000000..9f3b051
--- /dev/null
+++ b/Src/drivers/as5600/tests/test_as5600.cpp
@@ -0,0 +1,42 @@
+/**
+ * This program is free software under the GNU General Public License v3.
+ * See <https://www.gnu.org/licenses/> for details.
+ * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "drivers/as5600/as5600.hpp"
+
+TEST(AS5600Test, HelloWorld) {
+    EXPECT_EQ(1, 1);
+}
+
+TEST(AS5600Test, init) {
+    Driver::AS5600 as5600;
+    as5600.init();
+}
+
+TEST(AS5600Test, is_ready) {
+    Driver::AS5600 as5600;
+    as5600.is_ready();
+}
+
+TEST(AS5600Test, get_angle) {
+    Driver::AS5600 as5600;
+    as5600.get_angle();
+}
+
+TEST(AS5600Test, get_magnitude) {
+    Driver::AS5600 as5600;
+    as5600.get_magnitude();
+}
+
+TEST(AS5600Test, get_status) {
+    Driver::AS5600 as5600;
+    as5600.get_status();
+}
+
+int main(int argc, char **argv) {
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/Src/platform/ubuntu/i2c.cpp b/Src/platform/ubuntu/i2c.cpp
index f123e91..ebeae4d 100644
--- a/Src/platform/ubuntu/i2c.cpp
+++ b/Src/platform/ubuntu/i2c.cpp
@@ -6,3 +6,43 @@
 
 #include "peripheral/i2c/i2c.hpp"
 #include "main.h"
+
+namespace HAL {
+
+int8_t I2C::init() {
+    return -1;
+}
+
+int8_t I2C::is_device_ready(uint16_t address, uint8_t trials) {
+    (void)address;
+    (void)trials;
+    return -1;
+}
+
+int8_t I2C::transmit(uint16_t id, uint8_t tx[], uint8_t len) {
+    (void)id;
+    (void)tx;
+    (void)len;
+    return -1;
+}
+
+int8_t I2C::receive(uint16_t id, uint8_t *rx, uint8_t len) {
+    (void)id;
+    (void)rx;
+    (void)len;
+    return -1;
+}
+
+int32_t I2C::read_register_1_byte(uint16_t device_id, uint8_t reg_address) {
+    (void)device_id;
+    (void)reg_address;
+    return -1;
+}
+
+int32_t I2C::read_register_2_bytes(uint16_t device_id, uint8_t reg_address) {
+    (void)device_id;
+    (void)reg_address;
+    return -1;
+}
+
+}  // namespace HAL