diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt
index 32831e5..8cdf71a 100644
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -1,4 +1,4 @@
-idf_component_register(SRCS "main.c" "uart_handler.c" "communication_handler.c" "client_handler.c" "message_parser.c" "message_builder.c" "message_handler.c" "ota_update.c"
+idf_component_register(SRCS "main.c" "uart_handler.c" "communication_handler.c" "client_handler.c" "message_parser.c" "message_builder.c" "message_handler.c" "ota_update.c" "i2c.c"
                        INCLUDE_DIRS ".")
 
 # Get the short Git commit hash of the current HEAD.
diff --git a/main/i2c.c b/main/i2c.c
new file mode 100644
index 0000000..520543b
--- /dev/null
+++ b/main/i2c.c
@@ -0,0 +1,181 @@
+#include "i2c.h"
+#include "bma4.h"
+#include "bma456w.h"
+#include "bma4_defs.h"
+#include "driver/gpio.h"
+#include "driver/i2c_master.h"
+#include "esp_err.h"
+#include "esp_log.h"
+#include "freertos/idf_additions.h"
+#include "ota_update.h"
+#include <rom/ets_sys.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+static i2c_master_bus_handle_t bus_handle;
+static i2c_master_dev_handle_t bma456_dev_handle;
+static struct bma4_dev bma456_struct;
+
+#define BMA4_READ_WRITE_LEN UINT8_C(46)
+
+/******************************************************************************/
+/*!                User interface functions                                   */
+
+/*!
+ * I2C read function map to ESP platform
+ */
+BMA4_INTF_RET_TYPE bma4_i2c_read(uint8_t reg_addr, uint8_t *reg_data,
+                                 uint32_t len, void *intf_ptr) {
+  // ESP_ERROR_CHECK(i2c_master_receive(bma456_dev_handle, reg_data, len, -1));
+  esp_err_t err = i2c_master_transmit_receive(bma456_dev_handle, &reg_addr, 1,
+                                              reg_data, len, -1);
+  return (err == ESP_OK) ? BMA4_OK : BMA4_E_COM_FAIL;
+  // return BMA4_OK;
+}
+
+/*!
+ * I2C write function map to ESP platform
+ */
+BMA4_INTF_RET_TYPE bma4_i2c_write(uint8_t reg_addr, const uint8_t *reg_data,
+                                  uint32_t len, void *intf_ptr) {
+  // ESP_ERROR_CHECK(i2c_master_transmit(bma456_dev_handle, reg_data, len, -1));
+  // Bei Bosch muss zuerst das Register, dann die Daten in einem Transfer
+  // gesendet werden
+  uint8_t *buffer = malloc(len + 1);
+  if (!buffer)
+    return BMA4_E_NULL_PTR;
+
+  buffer[0] = reg_addr;
+
+  ESP_LOGI("I2CWrite", "Message Length: %d", len);
+
+  memcpy(&buffer[1], reg_data, len);
+
+  esp_err_t err = i2c_master_transmit(bma456_dev_handle, buffer, len + 1, -1);
+  free(buffer);
+
+  return (err == ESP_OK) ? BMA4_OK : BMA4_E_COM_FAIL;
+  // return BMA4_OK;
+}
+
+/*!
+ * Delay function map to ESP platform
+ */
+void bma4_delay_us(uint32_t period, void *intf_ptr) {
+  uint32_t wait_ms = period / 1000;
+  uint32_t wait_us = period % 1000;
+  if (wait_ms) {
+    vTaskDelay(pdMS_TO_TICKS(wait_ms));
+  }
+  ets_delay_us(wait_us);
+}
+
+/*!
+ *  @brief Prints the execution status of the APIs.
+ */
+void bma4_error_codes_print_result(const char api_name[], int8_t rslt) {
+  if (rslt != BMA4_OK) {
+    ESP_LOGI("BMA4_I2C", "%s\t", api_name);
+    if (rslt == BMA4_E_NULL_PTR) {
+      ESP_LOGI("BMA4_I2C", "Error [%d] : Null pointer\r\n", rslt);
+    } else if (rslt == BMA4_E_COM_FAIL) {
+      ESP_LOGI("BMA4_I2C", "Error [%d] : Communication failure\r\n", rslt);
+    } else if (rslt == BMA4_E_CONFIG_STREAM_ERROR) {
+      ESP_LOGI("BMA4_I2C", "Error [%d] : Invalid configuration stream\r\n",
+               rslt);
+    } else if (rslt == BMA4_E_SELF_TEST_FAIL) {
+      ESP_LOGI("BMA4_I2C", "Error [%d] : Self test failed\r\n", rslt);
+    } else if (rslt == BMA4_E_INVALID_SENSOR) {
+      ESP_LOGI("BMA4_I2C", "Error [%d] : Device not found\r\n", rslt);
+    } else if (rslt == BMA4_E_OUT_OF_RANGE) {
+      ESP_LOGI("BMA4_I2C", "Error [%d] : Out of Range\r\n", rslt);
+    } else if (rslt == BMA4_E_AVG_MODE_INVALID_CONF) {
+      ESP_LOGI("BMA4_I2C",
+               "Error [%d] : Invalid bandwidth and ODR combination in Accel "
+               "Averaging mode\r\n",
+               rslt);
+    } else {
+      /* For more error codes refer "*_defs.h" */
+      ESP_LOGI("BMA4_I2C", "Error [%d] : Unknown error code\r\n", rslt);
+    }
+  }
+}
+
+void init_i2c() {
+  i2c_master_bus_config_t i2c_mst_config = {
+      .clk_source = I2C_CLK_SRC_DEFAULT,
+      .i2c_port = I2C_PORT,
+      .scl_io_num = I2C_MASTER_SCL_IO,
+      .sda_io_num = I2C_MASTER_SDA_IO,
+      .glitch_ignore_cnt = 7,
+      //.flags.enable_internal_pullup = true,
+  };
+
+  ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_mst_config, &bus_handle));
+}
+
+void read_sensor_task(void *params) {
+  int8_t ret;
+  struct bma4_accel sens_data = {0};
+
+  while (1) {
+    ret = bma4_read_accel_xyz(&sens_data, &bma456_struct);
+    bma4_error_codes_print_result("bma4_read_accel_xyz", ret);
+
+    ESP_LOGI("i2c", "X:%d, Y%d, Z%d", sens_data.x, sens_data.y, sens_data.z);
+    vTaskDelay(pdMS_TO_TICKS(100));
+  }
+}
+
+void init_bma456() {
+  i2c_device_config_t dev_cfg_bma456 = {
+      .dev_addr_length = I2C_ADDR_BIT_LEN_7,
+      .device_address = BMA456_ADDRESS,
+      .scl_speed_hz = 100000,
+  };
+
+  ESP_ERROR_CHECK(i2c_master_bus_add_device(bus_handle, &dev_cfg_bma456,
+                                            &bma456_dev_handle));
+
+  bma456_struct.intf = BMA4_I2C_INTF;
+  bma456_struct.bus_read = bma4_i2c_read;
+  bma456_struct.bus_write = bma4_i2c_write;
+  bma456_struct.delay_us = bma4_delay_us;
+  bma456_struct.read_write_len = BMA4_READ_WRITE_LEN;
+  bma456_struct.intf_ptr = &bma456_dev_handle;
+
+  int8_t ret;
+  bma456_struct.chip_id = 0;
+
+  ret = bma456w_init(&bma456_struct);
+  bma4_error_codes_print_result("I2C Init", ret);
+
+  ESP_LOGI("I2C", "Chip Init ausgelesene CHIP ID %d", bma456_struct.chip_id);
+
+  ret = bma4_soft_reset(&bma456_struct);
+  bma4_error_codes_print_result("bma4_soft_reset", ret);
+  vTaskDelay(pdMS_TO_TICKS(20)); // Wartezeit nach Reset
+
+  ret = bma4_set_advance_power_save(BMA4_DISABLE, &bma456_struct);
+  bma4_error_codes_print_result("bma4_set_advance_power_save", ret);
+  vTaskDelay(pdMS_TO_TICKS(10));
+
+  ESP_LOGI("I2C", "Config SIZE %d", bma456_struct.config_size);
+  ESP_LOGI("I2C", "Config Pointer %p", bma456_struct.config_file_ptr);
+
+  ESP_LOGI("I2C", "Starte Config-File Upload...");
+  ret = bma456w_write_config_file(&bma456_struct);
+  bma4_error_codes_print_result("bma4_write_config_file", ret);
+
+  /* Enable the accelerometer */
+  ret = bma4_set_accel_enable(BMA4_ENABLE, &bma456_struct);
+  bma4_error_codes_print_result("bma4_set_accel_enable status", ret);
+
+  xTaskCreate(read_sensor_task, "READ_SENSOR", 4096, NULL, 1, NULL);
+}
+
+void init_i2c_with_all_devices() {
+  init_i2c();
+  init_bma456();
+}
diff --git a/main/i2c.h b/main/i2c.h
new file mode 100644
index 0000000..f278309
--- /dev/null
+++ b/main/i2c.h
@@ -0,0 +1,8 @@
+#define I2C_PORT 0
+#define I2C_MASTER_SCL_IO 5
+#define I2C_MASTER_SDA_IO 6
+#define BMA456_ADDRESS 0x18
+
+void init_i2c();
+void init_bma456();
+void init_i2c_with_all_devices();
diff --git a/main/main.c b/main/main.c
index 4b36b51..52f95c1 100644
--- a/main/main.c
+++ b/main/main.c
@@ -36,6 +36,8 @@
 #include "message_builder.h"
 #include "uart_msg_ids.h"
 
+#include "i2c.h"
+
 static const char *TAG = "ALOX - MAIN";
 static const uint16_t version = 0x0001;
 static uint8_t send_message_buffer[1024];
@@ -372,6 +374,8 @@ void app_main(void) {
     RegisterCallback(UART_CLIENT_INPUT, fakeDataCallback);
     RegisterCallback(UART_OTA_START_ESPNOW, start_ota_update_espnow);
 
+    init_i2c_with_all_devices();
+
   } else {
     ESP_LOGI(TAG, "Started in Slavemode");
     ESPNOW_RegisterSlaveCallbacks();