diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..9618221
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,12 @@
+{
+    "C_Cpp.errorSquiggles": "disabled",
+    "files.associations": {
+        "array": "cpp",
+        "deque": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "vector": "cpp",
+        "string_view": "cpp",
+        "initializer_list": "cpp"
+    }
+}
\ No newline at end of file
diff --git a/include/sensfuse.h b/include/sensfuse.h
new file mode 100644
index 0000000..3eeb6df
--- /dev/null
+++ b/include/sensfuse.h
@@ -0,0 +1,42 @@
+#ifndef __sensfuse__h__
+#define __sensfuse__h__
+#include <SPI.h>
+#include <Arduino.h>
+#include <Wire.h>
+#include <Adafruit_ISM330DHCX.h>
+
+// For SPI mode we need a CS pin
+#define LSM_CS PA4 
+// For software-SPI mode we need SCK/MOSI/MISO pins
+#define LSM_SCK D13
+#define LSM_MISO D12
+#define LSM_MOSI D11
+
+#define FUSION_STEP 0.05f // Fusion correct constant
+
+class SensorFusion {
+public: 
+    SensorFusion();
+
+    bool begin();
+    void update();
+
+    float getPitch();   
+    float getRoll();    
+
+private:
+    Adafruit_ISM330DHCX ism330dhcx;
+
+    unsigned long last_micros;
+
+    float ptch_accum;
+    float roll_accum; 
+
+    float fusion_correct_pitch;
+    float fusion_correct_roll;
+
+    float gyro_bias_x;
+    float gyro_bias_y;
+};
+
+#endif
\ No newline at end of file
diff --git a/platformio.ini b/platformio.ini
index 345e039..b60e17d 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -13,3 +13,4 @@ platform = ststm32
 board = nucleo_l432kc
 framework = arduino
 lib_extra_dirs = ./embedded-pio
+lib_deps = adafruit/Adafruit LSM6DS@^4.7.4
diff --git a/src/main.cpp b/src/main.cpp
index cb9fbba..552e2b2 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,18 +1,30 @@
-#include <Arduino.h>
+#include "sensfuse.h"
+#include <Adafruit_ISM330DHCX.h>
 
-// put function declarations here:
-int myFunction(int, int);
+SensorFusion fusion; // Create sensor fusion object
 
 void setup() {
-  // put your setup code here, to run once:
-  int result = myFunction(2, 3);
+    Serial.begin(115200);
+    while (!Serial)
+        delay(10); 
+
+    Serial.println("Adafruit ISM330DHCX test!");
+    if (!fusion.begin()) {
+        Serial.println("IMU initialization failed");
+        while (1) {
+            delay(10);
+        }
+    }
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+    fusion.update();
+
+    Serial.print("Pitch: ");
+    Serial.print(fusion.getPitch());
+    Serial.print("Roll: ");
+    Serial.println(fusion.getRoll());
+
+    delay(100);
 }
 
-// put function definitions here:
-int myFunction(int x, int y) {
-  return x + y;
-}
\ No newline at end of file
diff --git a/src/sensfuse.cpp b/src/sensfuse.cpp
new file mode 100644
index 0000000..b18d5fe
--- /dev/null
+++ b/src/sensfuse.cpp
@@ -0,0 +1,78 @@
+#include "sensfuse.h"
+
+SensorFusion::SensorFusion() 
+    : last_micros(0),
+      ptch_accum(0),
+      roll_accum(0),
+      fusion_correct_pitch(0),
+      fusion_correct_roll(0),
+      gyro_bias_x(0),
+      gyro_bias_y(0)
+{
+}
+
+bool SensorFusion::begin() {
+    if (!ism330dhcx.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) {
+        Serial.println("Failed to find ISM330DHCX chip1");
+        while (1) delay(10);
+    }
+
+    float sumX = 0.0f, sumY = 0.0f;
+    for (int i = 0; i < 500; i++) {
+        float gx, gy, gz;
+        ism330dhcx.readGyroscope(gx, gy, gz);
+        sumX += gx;
+        sumY += gy;
+        delay(2);
+    }
+
+    gyro_bias_x = sumX / 500.0f;
+    gyro_bias_y = sumY / 500.0f;
+
+    float ax, ay, az;
+    ism330dhcx.readAcceleration(ax, ay, az);
+
+    ptch_accum = atan2f(-ax, sqrtf(ay * ay + az * az)) * 57.2958f;
+    roll_accum = atan2f(ay, az) * 57.2958f;
+
+    last_micros = micros();
+
+    ism330dhcx.configInt1(false, false, true);
+    ism330dhcx.configInt2(false, true, false);
+
+    return true;
+}
+
+void SensorFusion::update() {
+    unsigned long now = micros();
+    float dt = (now - last_micros) * 1e-6f;
+    last_micros = now;
+
+    float gx, gy, gz;
+    float ax, ay, az;
+    ism330dhcx.readGyroscope(gx, gy, gz);
+    ism330dhcx.readAcceleration(ax, ay, az);
+
+    gx -= gyro_bias_x;
+    gy -= gyro_bias_y;
+
+    ptch_accum += (gx * dt) + fusion_correct_pitch;
+    roll_accum += (gy * dt) + fusion_correct_roll;
+
+    float ptch_acc_only = atan2f(-ax, sqrtf(ay * ay + az * az)) * 57.2958f;
+    float roll_acc_only = atan2f(ay, az) * 57.2958f;
+
+    if (ptch_acc_only > ptch_accum) fusion_correct_pitch += FUSION_STEP;
+    else if (ptch_acc_only < ptch_accum) fusion_correct_pitch -= FUSION_STEP;
+
+    if (roll_acc_only > roll_accum) fusion_correct_roll += FUSION_STEP;
+    else if (roll_acc_only < roll_accum) fusion_correct_roll -= FUSION_STEP;
+}
+
+float SensorFusion::getPitch() {
+    return ptch_accum;
+}
+
+float SensorFusion::getRoll() {
+    return roll_accum;
+}
\ No newline at end of file