diff --git a/apps/ec/ServoService/servoController/servo_controller.cpp b/apps/ec/ServoService/servoController/servo_controller.cpp
index 9406471e..466c3283 100644
--- a/apps/ec/ServoService/servoController/servo_controller.cpp
+++ b/apps/ec/ServoService/servoController/servo_controller.cpp
@@ -38,11 +38,12 @@ void ServoController::closingThreadLoop(const std::stop_token& token) {
         if (cfg.value().auto_closing == 0) {
           continue;
         }
-        if (cfg.value().position != kOpenState) {
+        if (cfg.value().position == kCloseState) {
           continue;
         }
         if (cfg.value().open_time_end <= now) {
           servo_ctr_.SetServoPosition(cfg.value(), 0);
+          servo_cfg_mng.SetServoPosition(id, kCloseState);
         } else {
           auto time_until_end = std::chrono::duration_cast<
                   std::chrono::milliseconds>(cfg.value().open_time_end - now);
@@ -67,17 +68,17 @@ void ServoController::pulsingThreadLoop(const std::stop_token& token) {
       if (cfg.value().pulsing_time == 0) {
         continue;
       }
-      if (cfg.value().position == kCloseState) {
-        pulsing_db.erase(id);
-        servo_ctr_.SetServoPosition(cfg.value(), kCloseState);
-        continue;
-      }
       {
         std::lock_guard<std::mutex> lock(pulsing_mtx_);
         auto pulse_cfg = pulsing_db.find(id);
         if (pulse_cfg == pulsing_db.end()) {
           continue;
         }
+        if (cfg.value().position == kCloseState) {
+          pulsing_db.erase(id);
+          servo_ctr_.SetServoPosition(cfg.value(), kCloseState);
+          continue;
+        }
         auto now = Clock::now();
         if (now >= pulse_cfg->second.pulse_deadline) {
           pulse_cfg->second.pulse_state = !pulse_cfg->second.pulse_state;
@@ -155,8 +156,6 @@ bool ServoController::AutoSetServoPosition(const uint8_t actuator_id,
     logger_.LogDebug() << "ServoController.AutoSetServoPosition: enabled pulsing for actuator "
                        << actuator_id << ", interval_ms " << cfg.value().pulsing_time;
   }
-
-
   return true;
 }
 
diff --git a/apps/ec/ServoService/servoController/servo_driver.hpp b/apps/ec/ServoService/servoController/servo_driver.hpp
index b95c96e6..8cd5ff8c 100644
--- a/apps/ec/ServoService/servoController/servo_driver.hpp
+++ b/apps/ec/ServoService/servoController/servo_driver.hpp
@@ -50,7 +50,8 @@ class ServoDriver {
     if (cfg.has_value()) {
       config = cfg.value();
     } else {
-      ara::log::LogWarn() << "Cant get Eeprom corelation data";
+      ara::log::LogFatal() << "Cant get Eeprom corelation data";
+      exit(1);
     }
   }
   ServoDriver():
diff --git a/apps/ec/logger_service/data_type.cpp b/apps/ec/logger_service/data_type.cpp
index b177b95d..b833fca3 100644
--- a/apps/ec/logger_service/data_type.cpp
+++ b/apps/ec/logger_service/data_type.cpp
@@ -2,18 +2,17 @@
  * @file data_type.cpp
  * @author Mateusz Krajewski (matikrajek42@gmail.com)
  * @brief 
- * @version 0.1
- * @date 2024-11-22
- * 
- * @copyright Copyright (c) 2024
- * 
- */
-
+ * @version 0.2
+ * @date 2026-05-25
+ * * @copyright Copyright (c) 2024-2026
+ * */
 
 #include "apps/ec/logger_service/data_type.hpp"
 #include <sstream>
 #include <iomanip>
 #include <cstring>
+#include "ara/log/log.h"
+
 namespace srp {
 namespace logger {
 
@@ -22,12 +21,7 @@ namespace {
                   "BOARD_TEMP2;BOARD_TEMP3;TANK_PRESS;"
                   "TANK_D_PRESS;CPU_USAGE;MEM_USAGE;DISK_UTILIZATION;TENSO;"
                   "PRIMER_STATUS;SERVO_STATUS;SERVO_DUMP_STATUS;"
-                  "SERVO_VENT_STATUS;ENGINE_MODE;ENGINE_NEW_VENT_VALVE_STATUS";
-}
-
-void Data_t::SetTenso(const tensoType& tenso) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->tenso = tenso;
+                  "SERVO_VENT_STATUS;ENGINE_MODE;ENGINE_NEW_VENT_VALVE_STATUS;GPIO_STATE";
 }
 
 std::string Data_t::get_header() {
@@ -35,42 +29,29 @@ std::string Data_t::get_header() {
 }
 
 std::vector<uint8_t> Data_t::get_bytes(const int64_t& timestamp) {
-  tempType temp1_local;
-  tempType temp2_local;
-  tempType temp3_local;
-  tempType board_temp1_local;
-  tempType board_temp2_local;
-  tempType board_temp3_local;
-  pressType tank_press_local;
-  dPressType tank_d_press_local;
-  apps::SysStatType sys_status_local;
-  tensoType tenso_local;
-  primerStatusType primer_status_local;
-  servoType servo_status_local;
-  servoType servo_dump_status_local;
-  servoType servo_vent_status_local;
-  engineType engine_mode_local;
-  engineType engine_new_vent_valve_status_local;
-
-  {
-    std::unique_lock<std::shared_mutex> lock(this->mutex_);
-    temp1_local = temp1;
-    temp2_local = temp2;
-    temp3_local = temp3;
-    board_temp1_local = board_temp1;
-    board_temp2_local = board_temp2;
-    board_temp3_local = board_temp3;
-    tank_press_local = tank_press;
-    tank_d_press_local = tank_d_press;
-    sys_status_local = sys_status;
-    tenso_local = tenso;
-    primer_status_local = primer_status;
-    servo_status_local = servo_status;
-    servo_dump_status_local = servo_dump_status;
-    servo_vent_status_local = servo_vent_status;
-    engine_mode_local = engine_mode;
-    engine_new_vent_valve_status_local = engine_new_vent_valve_status;
-  }
+  // Pobieramy wszystkie wartości lokalnie na starcie metodą relaxed
+  const tempType temp1_local = temp1.load(std::memory_order_relaxed);
+  const tempType temp2_local = temp2.load(std::memory_order_relaxed);
+  const tempType temp3_local = temp3.load(std::memory_order_relaxed);
+  const tempType board_temp1_local = board_temp1.load(std::memory_order_relaxed);
+  const tempType board_temp2_local = board_temp2.load(std::memory_order_relaxed);
+  const tempType board_temp3_local = board_temp3.load(std::memory_order_relaxed);
+  const pressType tank_press_local = tank_press.load(std::memory_order_relaxed);
+  const dPressType tank_d_press_local = tank_d_press.load(std::memory_order_relaxed);
+
+  const auto cpu_local = sys_cpu_usage.load(std::memory_order_relaxed);
+  const auto mem_local = sys_mem_usage.load(std::memory_order_relaxed);
+  const auto disk_local = sys_disk_utilization.load(std::memory_order_relaxed);
+
+  const tensoType tenso_local = tenso.load(std::memory_order_relaxed);
+  const primerStatusType primer_status_local = primer_status.load(std::memory_order_relaxed);
+  const servoType servo_status_local = servo_status.load(std::memory_order_relaxed);
+  const servoType servo_dump_status_local = servo_dump_status.load(std::memory_order_relaxed);
+  const servoType servo_vent_status_local = servo_vent_status.load(std::memory_order_relaxed);
+  const engineType engine_mode_local = engine_mode.load(std::memory_order_relaxed);
+  const engineType engine_new_vent_valve_status_local = engine_new_vent_valve_status.load(std::memory_order_relaxed);
+
+  const uint32_t gpio_ = gpio_states.load(std::memory_order_relaxed);
 
   constexpr std::size_t kTotalSize =
       sizeof(timestamp) +
@@ -82,16 +63,17 @@ std::vector<uint8_t> Data_t::get_bytes(const int64_t& timestamp) {
       sizeof(board_temp3_local) +
       sizeof(tank_press_local) +
       sizeof(tank_d_press_local) +
-      sizeof(sys_status_local.cpu_usage) +
-      sizeof(sys_status_local.mem_usage) +
-      sizeof(sys_status_local.disk_utilization) +
+      sizeof(cpu_local) +
+      sizeof(mem_local) +
+      sizeof(disk_local) +
       sizeof(tenso_local) +
       sizeof(primer_status_local) +
       sizeof(servo_status_local) +
       sizeof(servo_dump_status_local) +
       sizeof(servo_vent_status_local) +
       sizeof(engine_mode_local) +
-      sizeof(engine_new_vent_valve_status_local);
+      sizeof(engine_new_vent_valve_status_local) +
+      sizeof(gpio_);
 
   std::vector<uint8_t> bytes;
   bytes.resize(kTotalSize);
@@ -113,9 +95,9 @@ std::vector<uint8_t> Data_t::get_bytes(const int64_t& timestamp) {
   append_bytes(offset, board_temp3_local);
   append_bytes(offset, tank_press_local);
   append_bytes(offset, tank_d_press_local);
-  append_bytes(offset, sys_status_local.cpu_usage);
-  append_bytes(offset, sys_status_local.mem_usage);
-  append_bytes(offset, sys_status_local.disk_utilization);
+  append_bytes(offset, cpu_local);
+  append_bytes(offset, mem_local);
+  append_bytes(offset, disk_local);
   append_bytes(offset, tenso_local);
   append_bytes(offset, primer_status_local);
   append_bytes(offset, servo_status_local);
@@ -123,96 +105,139 @@ std::vector<uint8_t> Data_t::get_bytes(const int64_t& timestamp) {
   append_bytes(offset, servo_vent_status_local);
   append_bytes(offset, engine_mode_local);
   append_bytes(offset, engine_new_vent_valve_status_local);
+  append_bytes(offset, gpio_);
 
   return bytes;
 }
 
 std::string Data_t::to_string(const std::string& timestamp) {
+  // Pobieramy lokalne kopie wszystkich zmiennych na początku funkcji
+  const tempType temp1_local = temp1.load(std::memory_order_relaxed);
+  const tempType temp2_local = temp2.load(std::memory_order_relaxed);
+  const tempType temp3_local = temp3.load(std::memory_order_relaxed);
+  const tempType board_temp1_local = board_temp1.load(std::memory_order_relaxed);
+  const tempType board_temp2_local = board_temp2.load(std::memory_order_relaxed);
+  const tempType board_temp3_local = board_temp3.load(std::memory_order_relaxed);
+  const pressType tank_press_local = tank_press.load(std::memory_order_relaxed);
+  const dPressType tank_d_press_local = tank_d_press.load(std::memory_order_relaxed);
+
+  const auto cpu_local = sys_cpu_usage.load(std::memory_order_relaxed);
+  const auto mem_local = sys_mem_usage.load(std::memory_order_relaxed);
+  const auto disk_local = sys_disk_utilization.load(std::memory_order_relaxed);
+
+  const tensoType tenso_local = tenso.load(std::memory_order_relaxed);
+  const auto primer_local = static_cast<int>(primer_status.load(std::memory_order_relaxed));
+  const auto servo_local = static_cast<int>(servo_status.load(std::memory_order_relaxed));
+  const auto servo_dump_local = static_cast<int>(servo_dump_status.load(std::memory_order_relaxed));
+  const auto servo_vent_local = static_cast<int>(servo_vent_status.load(std::memory_order_relaxed));
+  const auto engine_mode_local = static_cast<int>(engine_mode.load(std::memory_order_relaxed));
+  const auto engine_vent_valve_local = static_cast<int>(engine_new_vent_valve_status.load(std::memory_order_relaxed));
+
+  const uint32_t gpio_ = gpio_states.load(std::memory_order_relaxed);
+
   std::stringstream res;
-  res << std::fixed << std::setprecision(2);
+  res << std::fixed << std::setprecision(4);
   res << timestamp << ";";
-  std::shared_lock<std::shared_mutex> lock(this->mutex_);
-  res << temp1 << ";";
-  res << temp2 << ";";
-  res << temp3 << ";";
-  res << board_temp1 << ";";
-  res << board_temp2 << ";";
-  res << board_temp3 << ";";
-  res << tank_press << ";";
-  res << tank_d_press << ";";
-  res << sys_status.cpu_usage << ";";
-  res << sys_status.mem_usage << ";";
-  res << sys_status.disk_utilization << ";";
-  res << tenso << ";";
-  res << static_cast<unsigned>(primer_status) << ";";
-  res << static_cast<unsigned>(servo_status) << ";";
-  res << static_cast<unsigned>(servo_dump_status) << ";";
-  res << static_cast<unsigned>(servo_vent_status) << ";";
-  res << static_cast<unsigned>(engine_mode) << ";";
-  res << static_cast<unsigned>(engine_new_vent_valve_status);
+  res << temp1_local << ";";
+  res << temp2_local << ";";
+  res << temp3_local << ";";
+  res << board_temp1_local << ";";
+  res << board_temp2_local << ";";
+  res << board_temp3_local << ";";
+  res << tank_press_local << ";";
+  res << tank_d_press_local << ";";
+  res << cpu_local << ";";
+  res << mem_local << ";";
+  res << disk_local << ";";
+  res << tenso_local << ";";
+  res << primer_local << ";";
+  res << servo_local << ";";
+  res << servo_dump_local << ";";
+  res << servo_vent_local << ";";
+  res << engine_mode_local << ";";
+  res << engine_vent_valve_local << ";";
+  res << gpio_;
   return res.str();
 }
 
 void Data_t::SetSysStatus(const apps::SysStatType& sys_stat) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->sys_status = sys_stat;
+  this->sys_cpu_usage.store(sys_stat.cpu_usage, std::memory_order_relaxed);
+  this->sys_mem_usage.store(sys_stat.mem_usage, std::memory_order_relaxed);
+  this->sys_disk_utilization.store(sys_stat.disk_utilization, std::memory_order_relaxed);
+}
+
+void Data_t::SetGpioState(const uint8_t pin_id, const uint8_t state) {
+  if (pin_id >= 32) {
+    ara::log::LogWarn() << "GPIO pin_id out of range: " << static_cast<int>(pin_id);
+    return;
+  }
+  const uint32_t mask = 1U << pin_id;
+  if (state == 0) {
+    gpio_states.fetch_and(~mask, std::memory_order_relaxed);
+  } else {
+    gpio_states.fetch_or(mask, std::memory_order_relaxed);
+  }
+}
+
+
+void Data_t::SetTenso(tensoType tenso) {
+  this->tenso.store(tenso, std::memory_order_relaxed);
 }
 
-void Data_t::SetTemp1(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->temp1 = temp;
+void Data_t::SetTemp1(tempType temp) {
+  this->temp1.store(temp, std::memory_order_relaxed);
 }
-void Data_t::SetTemp2(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->temp2 = temp;
+
+void Data_t::SetTemp2(tempType temp) {
+  this->temp2.store(temp, std::memory_order_relaxed);
 }
-void Data_t::SetTemp3(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->temp3 = temp;
+
+void Data_t::SetTemp3(tempType temp) {
+  this->temp3.store(temp, std::memory_order_relaxed);
 }
-void Data_t::SetBoardTemp1(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->board_temp1 = temp;
+
+void Data_t::SetBoardTemp1(tempType temp) {
+  this->board_temp1.store(temp, std::memory_order_relaxed);
 }
-void Data_t::SetBoardTemp2(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->board_temp2 = temp;
+
+void Data_t::SetBoardTemp2(tempType temp) {
+  this->board_temp2.store(temp, std::memory_order_relaxed);
 }
-void Data_t::SetBoardTemp3(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->board_temp3 = temp;
+
+void Data_t::SetBoardTemp3(tempType temp) {
+  this->board_temp3.store(temp, std::memory_order_relaxed);
 }
-void Data_t::SetTankPress(const pressType& press) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->tank_press = press;
+
+void Data_t::SetTankPress(pressType press) {
+  this->tank_press.store(press, std::memory_order_relaxed);
 }
-void Data_t::SetTankDPress(const dPressType& press) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->tank_d_press = press;
+
+void Data_t::SetTankDPress(dPressType press) {
+  this->tank_d_press.store(press, std::memory_order_relaxed);
 }
-void Data_t::SetPrimerStatus(const primerStatusType& primer) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->primer_status = primer;
+
+void Data_t::SetPrimerStatus(primerStatusType primer) {
+  this->primer_status.store(primer, std::memory_order_relaxed);
 }
-void Data_t::SetServoStatus(const servoType& status) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->servo_status = status;
+
+void Data_t::SetServoStatus(servoType status) {
+  this->servo_status.store(status, std::memory_order_relaxed);
 }
-void Data_t::SetServoDumpStatus(const servoType& status) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->servo_dump_status = status;
+
+void Data_t::SetServoDumpStatus(servoType status) {
+  this->servo_dump_status.store(status, std::memory_order_relaxed);
 }
-void Data_t::SetServoVentStatus(const servoType& status) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->servo_vent_status = status;
+
+void Data_t::SetServoVentStatus(servoType status) {
+  this->servo_vent_status.store(status, std::memory_order_relaxed);
 }
-void Data_t::SetEngineMode(const engineType& mode) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->engine_mode = mode;
+
+void Data_t::SetEngineMode(engineType mode) {
+  this->engine_mode.store(mode, std::memory_order_relaxed);
 }
-void Data_t::SetNewVentValveStatus(const engineType& status) {
-  std::unique_lock<std::shared_mutex> lock(this->mutex_);
-  this->engine_new_vent_valve_status = status;
+
+void Data_t::SetNewVentValveStatus(engineType status) {
+  this->engine_new_vent_valve_status.store(status, std::memory_order_relaxed);
 }
 
 }  // namespace logger
diff --git a/apps/ec/logger_service/data_type.hpp b/apps/ec/logger_service/data_type.hpp
index 730c13aa..88f4ffa5 100644
--- a/apps/ec/logger_service/data_type.hpp
+++ b/apps/ec/logger_service/data_type.hpp
@@ -2,18 +2,15 @@
  * @file data_type.hpp
  * @author Mateusz Krajewski (matikrajek42@gmail.com)
  * @brief 
- * @version 0.1
- * @date 2024-11-22
- * 
- * @copyright Copyright (c) 2024
- * 
- */
+ * @version 0.2
+ * @date 2026-05-25
+ * * @copyright Copyright (c) 2024-2026
+ * */
 
 #ifndef APPS_EC_LOGGER_SERVICE_DATA_TYPE_HPP_
 #define APPS_EC_LOGGER_SERVICE_DATA_TYPE_HPP_
 
-#include <shared_mutex>
-#include <mutex>  // NOLINT
+#include <atomic>
 #include <cstdint>
 #include <vector>
 #include <memory>
@@ -32,44 +29,55 @@ class Data_t {
   using primerStatusType = uint8_t;
   using servoType = uint8_t;
   using engineType = uint8_t;
-  std::shared_mutex mutex_;
-  tensoType tenso;
-  tempType temp1;
-  tempType temp2;
-  tempType temp3;
-  tempType board_temp1;
-  tempType board_temp2;
-  tempType board_temp3;
-  pressType tank_press;
-  dPressType tank_d_press;
-  primerStatusType primer_status;
-  servoType servo_status;
-  servoType servo_dump_status;
-  servoType servo_vent_status;
-  engineType engine_mode;
-  engineType engine_new_vent_valve_status;
-  apps::SysStatType sys_status;
+
+  std::atomic<tensoType> tenso{0.0f};
+  std::atomic<tempType> temp1{0};
+  std::atomic<tempType> temp2{0};
+  std::atomic<tempType> temp3{0};
+  std::atomic<tempType> board_temp1{0};
+  std::atomic<tempType> board_temp2{0};
+  std::atomic<tempType> board_temp3{0};
+  std::atomic<pressType> tank_press{0.0f};
+  std::atomic<dPressType> tank_d_press{0.0f};
+  std::atomic<primerStatusType> primer_status{0};
+  std::atomic<servoType> servo_status{0};
+  std::atomic<servoType> servo_dump_status{0};
+  std::atomic<servoType> servo_vent_status{0};
+  std::atomic<engineType> engine_mode{0};
+  std::atomic<engineType> engine_new_vent_valve_status{0};
+  std::atomic<float> sys_cpu_usage{0.0f};
+  std::atomic<float> sys_mem_usage{0.0f};
+  std::atomic<float> sys_disk_utilization{0.0f};
+  std::atomic<uint32_t> gpio_states{0};
 
  public:
+  Data_t() = default;
+
+  Data_t(const Data_t&) = delete;
+  Data_t& operator=(const Data_t&) = delete;
+
   std::string get_header();
   std::string to_string(const std::string& timestamp);
   std::vector<uint8_t> get_bytes(const int64_t& timestamp);
+
+  void SetGpioState(const uint8_t pin_id, const uint8_t state);
+
   void SetSysStatus(const apps::SysStatType& sys_stat);
-  void SetTemp1(const tempType& temp);
-  void SetTemp2(const tempType& temp);
-  void SetTemp3(const tempType& temp);
-  void SetBoardTemp1(const tempType& temp);
-  void SetBoardTemp2(const tempType& temp);
-  void SetBoardTemp3(const tempType& temp);
-  void SetTankPress(const pressType& press);
-  void SetTankDPress(const dPressType& press);
-  void SetTenso(const tensoType& tenso);
-  void SetPrimerStatus(const primerStatusType& primer);
-  void SetServoStatus(const servoType& status);
-  void SetServoDumpStatus(const servoType& status);
-  void SetServoVentStatus(const servoType& status);
-  void SetEngineMode(const engineType& mode);
-  void SetNewVentValveStatus(const engineType& status);
+  void SetTemp1(tempType temp);
+  void SetTemp2(tempType temp);
+  void SetTemp3(tempType temp);
+  void SetBoardTemp1(tempType temp);
+  void SetBoardTemp2(tempType temp);
+  void SetBoardTemp3(tempType temp);
+  void SetTankPress(pressType press);
+  void SetTankDPress(dPressType press);
+  void SetTenso(tensoType tenso);
+  void SetPrimerStatus(primerStatusType primer);
+  void SetServoStatus(servoType status);
+  void SetServoDumpStatus(servoType status);
+  void SetServoVentStatus(servoType status);
+  void SetEngineMode(engineType mode);
+  void SetNewVentValveStatus(engineType status);
 };
 
 }  // namespace logger
diff --git a/apps/ec/logger_service/service/logger_service.cpp b/apps/ec/logger_service/service/logger_service.cpp
index a79d525e..ba4ac541 100644
--- a/apps/ec/logger_service/service/logger_service.cpp
+++ b/apps/ec/logger_service/service/logger_service.cpp
@@ -25,7 +25,7 @@ namespace logger {
 namespace {
   static constexpr std::string kloger_filename = "_log.csv";
   static constexpr std::string kloger_filename_prefix = "/home/root/";
-  static constexpr uint16_t kSave_interval = 5;
+  static constexpr uint16_t kSave_interval = 10;
   static constexpr auto kEnv_service_path_name = "srp/apps/FileLoggerApp/EnvApp";
   static constexpr auto kUdp_service_path_name = "srp/apps/FileLoggerApp/logService_udp";
   static constexpr auto kIpc_service_path_name = "srp/apps/FileLoggerApp/logService_ipc";
@@ -37,6 +37,8 @@ namespace {
   static constexpr auto kLogs_on = 1;
   static constexpr auto kLogs_off = 0;
   static constexpr auto kHeartBeatPinID = 2;
+  static constexpr auto gpios_sub = {1, 2, 3, 4, 5, 6, 7, 8, 9,
+                10, 11, 12, 13, 14, 15, 16, 17};
 }  // namespace
 void LoggerService::SaveLoop(const std::stop_token& token,
             std::shared_ptr<core::timestamp::TimestampController> timestamp) {
@@ -107,6 +109,14 @@ int LoggerService::Initialize(const std::map<ara::core::StringView, ara::core::S
   service_ipc->StartOffer();
   service_udp->StartOffer();
   this->SomeIpInit();
+  gpio_.SetCallback([this](uint8_t pin_id, uint8_t state) {
+    data.SetGpioState(pin_id, state);
+  });
+  for (const auto& id: gpios_sub) {
+    if (gpio_.ManagePinSubscription(id, true) != core::ErrorCode::kOk) {
+      ara::log::LogError() << "Failed to subscribe pin id: " << static_cast<int>(id);
+    }
+  }
   return 0;
 }
 
diff --git a/apps/ec/logger_service/tests/data_type_test.cc b/apps/ec/logger_service/tests/data_type_test.cc
index 28f56810..834cec73 100644
--- a/apps/ec/logger_service/tests/data_type_test.cc
+++ b/apps/ec/logger_service/tests/data_type_test.cc
@@ -9,9 +9,37 @@
  * 
  */
 #include <gtest/gtest.h>
+#include <cstring>
 
 #include "apps/ec/logger_service/data_type.hpp"
 
+namespace {
+
+uint32_t GpioStateFromBytes(const std::vector<uint8_t>& bytes) {
+  uint32_t gpio = 0;
+  ASSERT_GE(bytes.size(), sizeof(gpio));
+  std::memcpy(&gpio, bytes.data() + bytes.size() - sizeof(gpio), sizeof(gpio));
+  return gpio;
+}
+
+}  // namespace
+
+TEST(DataGpioStateTest, SupportsPinIdsAboveEight) {
+  srp::logger::Data_t data;
+  data.SetGpioState(12, 1);
+  EXPECT_EQ(GpioStateFromBytes(data.get_bytes(0)), 1U << 12);
+}
+
+TEST(DataGpioStateTest, ClearingOnePinKeepsOthers) {
+  srp::logger::Data_t data;
+  data.SetGpioState(2, 1);
+  data.SetGpioState(3, 1);
+  EXPECT_EQ(GpioStateFromBytes(data.get_bytes(0)), 12U);
+
+  data.SetGpioState(2, 0);
+  EXPECT_EQ(GpioStateFromBytes(data.get_bytes(0)), 8U);
+}
+
 // TEST(DataToStringTest, DataToStringTests) {
 //     srp::logger::Data_t data_;
 //     data_.SetTemp1(1);
@@ -22,4 +50,3 @@
 //     data_.SetSysStatus({5.0f, 6.0f, 7.0f});
 //     EXPECT_EQ(data_.to_string("12"), "12;1;2;3;4.00;1.22;6.00;5.00;7.00");
 // }
-
diff --git a/apps/ec/radio_service/someip_controller.cpp b/apps/ec/radio_service/someip_controller.cpp
index 98613d28..c3fe568f 100644
--- a/apps/ec/radio_service/someip_controller.cpp
+++ b/apps/ec/radio_service/someip_controller.cpp
@@ -19,14 +19,14 @@ namespace apps {
 namespace radio {
 
 namespace {
-  static constexpr auto kEnv_service_path_name =      "srp/apps/FcRadioApp/EnvApp";
-  static constexpr auto kGPS_service_path_name =      "srp/apps/FcRadioApp/GPSService";
-  static constexpr auto kPrimer_service_path_name =   "srp/apps/FcRadioApp/PrimerService";
-  static constexpr auto kServo_service_path_name =    "srp/apps/FcRadioApp/ServoService";
-  static constexpr auto kRecovery_service_path_name = "srp/apps/FcRadioApp/RecoveryService";
-  static constexpr auto kMain_service_path_name =     "srp/apps/FcRadioApp/MainService";
-  static constexpr auto kEngine_service_path_name =   "srp/apps/FcRadioApp/EngineService";
-  static constexpr auto kEnv_fc_service_path_name =   "srp/apps/FcRadioApp/EnvAppFc";
+  static constexpr auto kEnv_service_path_name =      "srp/apps/RadioApp/EnvApp";
+  static constexpr auto kGPS_service_path_name =      "srp/apps/RadioApp/GPSService";
+  static constexpr auto kPrimer_service_path_name =   "srp/apps/RadioApp/PrimerService";
+  static constexpr auto kServo_service_path_name =    "srp/apps/RadioApp/ServoService";
+  static constexpr auto kRecovery_service_path_name = "srp/apps/RadioApp/RecoveryService";
+  static constexpr auto kMain_service_path_name =     "srp/apps/RadioApp/MainService";
+  static constexpr auto kEngine_service_path_name =   "srp/apps/RadioApp/EngineService";
+  static constexpr auto kEnv_fc_service_path_name =   "srp/apps/RadioApp/EnvAppFc";
 }  // namespace
 
 SomeIPController::SomeIPController():
diff --git a/apps/ec/system_stat_service/system_stat_service.cpp b/apps/ec/system_stat_service/system_stat_service.cpp
index a28cfa56..d785d00f 100644
--- a/apps/ec/system_stat_service/system_stat_service.cpp
+++ b/apps/ec/system_stat_service/system_stat_service.cpp
@@ -17,7 +17,6 @@
 #include "apps/ec/system_stat_service/system_stat_service.hpp"
 #include "core/common/condition.h"
 #include "ara/log/log.h"
-#include "core/sys/system_stat.hpp"
 
 namespace srp {
 namespace sysService {
@@ -43,17 +42,17 @@ int SystemStatService::Initialize(const std::map<ara::core::StringView, ara::cor
 
 std::optional<apps::SysStatType> SystemStatService::GetSysStats() const {
     apps::SysStatType stats;
-    auto cpu_usage_opt = core::stat::SystemStats::get_cpu_usage();
+    auto cpu_usage_opt = stats_.get_cpu_usage();
     if (!cpu_usage_opt.has_value()) {
         return std::nullopt;
     }
     stats.cpu_usage = static_cast<float>(cpu_usage_opt.value());
-    auto mem_usage_opt = core::stat::SystemStats::get_ram_usage();
+    auto mem_usage_opt = stats_.get_ram_usage();
     if (!mem_usage_opt.has_value()) {
         return std::nullopt;
     }
     stats.mem_usage = mem_usage_opt.value();
-    stats.disk_utilization = static_cast<float>(core::stat::SystemStats::get_disk_space());
+    stats.disk_utilization = static_cast<float>(stats_.get_disk_space());
     return stats;
 }
 
@@ -71,7 +70,7 @@ int SystemStatService::Run(const std::stop_token& token) {
                             << "%, mem usage: " << stats.mem_usage
                             << "%, disk usage: " << stats.disk_utilization << "%";
 
-        ara::log::LogDebug() << ss.str();
+        ara::log::LogWarn() << ss.str();
         service_ipc.NewSystemUsage.Update(stats);
         service_udp.NewSystemUsage.Update(stats);
     }
diff --git a/apps/ec/system_stat_service/system_stat_service.hpp b/apps/ec/system_stat_service/system_stat_service.hpp
index 02dfeb93..f9913288 100644
--- a/apps/ec/system_stat_service/system_stat_service.hpp
+++ b/apps/ec/system_stat_service/system_stat_service.hpp
@@ -19,6 +19,7 @@
 #include <utility>
 #include "ara/exec/adaptive_application.h"
 #include "srp/apps/SysStatServiceSkeleton.h"
+#include "core/sys/system_stat.hpp"
 
 namespace srp {
 namespace sysService {
@@ -28,6 +29,8 @@ class SystemStatService final : public ara::exec::AdaptiveApplication {
   apps::SysStatServiceSkeleton service_ipc;
   apps::SysStatServiceSkeleton service_udp;
 
+  core::stat::SystemStats stats_;
+  
   std::optional<apps::SysStatType> GetSysStats() const;
 
  protected:
diff --git a/apps/fc/env_service/BUILD b/apps/fc/env_service/BUILD
index 450fce31..50857eb5 100644
--- a/apps/fc/env_service/BUILD
+++ b/apps/fc/env_service/BUILD
@@ -31,5 +31,6 @@ cc_binary(
         "//mw/i2c_service/controller/bme280:bme280_controller",
         "//core/json:simba_json",
         "//mw/i2c_service/controller/24lc32at/eeprom_config:eeprom_config",
+        "//mw/i2c_service/controller/LSM6DSOX:LSM6DSOX_controller",
     ],
 )
\ No newline at end of file
diff --git a/apps/fc/env_service/env_service.cpp b/apps/fc/env_service/env_service.cpp
index f7280a22..ec5feee9 100644
--- a/apps/fc/env_service/env_service.cpp
+++ b/apps/fc/env_service/env_service.cpp
@@ -8,6 +8,7 @@
  * @copyright Copyright (c) 2026
  * 
  */
+#include <cstring>
 #include <memory>
 #include <map>
 #include <string>
@@ -41,6 +42,10 @@ core::ErrorCode EnvServiceFc::Init(std::unique_ptr<mw::temp::TempController> tem
       return core::ErrorCode::kInitializeError;
     }
     this->temp_ = std::move(temp);
+    if (config.Init() != core::ErrorCode::kOk) {
+        ara::log::LogError() << "EnvServiceFc::Init: EEPROM ConfigManager init failed";
+        return core::ErrorCode::kInitializeError;
+    }
     return core::ErrorCode::kOk;
 }
 
@@ -54,7 +59,7 @@ int EnvServiceFc::Initialize(const std::map<ara::core::StringView, ara::core::St
         ara::log::LogError() << "app_path parameter not found in parms";
         return core::ErrorCode::kInitializeError;
     }
-    config.Init();
+    std::this_thread::sleep_for(std::chrono::seconds(3));
     // Initialize pressure sensor
     bme = std::make_shared<i2c::BME280>();
     auto i2c = std::make_unique<i2c::I2CController>();
@@ -81,7 +86,8 @@ int EnvServiceFc::Initialize(const std::map<ara::core::StringView, ara::core::St
         res = this->temp_->Initialize(kSomeIPServiceID, std::bind(&EnvServiceFc::TempRxCallback,
             this, std::placeholders::_1), std::make_unique<com::soc::StreamIpcSocket>());
         if (res != core::ErrorCode::kOk) {
-            ara::log::LogInfo() << "Cant connect to temp by id:" << kSomeIPServiceID << ", try num: " << i;
+            ara::log::LogWarn() << "TempController init failed for id:" << kSomeIPServiceID
+                                << ", try num: " << i;
         }
         i++;
     } while (res != core::ErrorCode::kOk && i < 6);
@@ -93,7 +99,7 @@ int EnvServiceFc::Initialize(const std::map<ara::core::StringView, ara::core::St
         ara::log::LogError() << "Failed to load temperature configuration";
         return core::ErrorCode::kInitializeError;
     }
-    //     eeprom::EEPROM_config cfg{};
+    // eeprom::EEPROM_config cfg{};
     // cfg.pca9685_XO_corelation = 1.0;
     // strncpy(cfg.board_temp1_id, "000011106551", sizeof(cfg.board_temp1_id));
     // strncpy(cfg.board_temp2_id, "00001110e419", sizeof(cfg.board_temp2_id));
@@ -117,8 +123,10 @@ int EnvServiceFc::Initialize(const std::map<ara::core::StringView, ara::core::St
         return core::ErrorCode::kInitializeError;
     }
 
-    auto register_sensor = [&](const std::string& raw_id, const std::string& label) -> core::ErrorCode {
-        std::string physical_id = "28-" + raw_id;
+    auto register_sensor = [&](const char* raw_id, std::size_t raw_id_max_len,
+                               const std::string& label) -> core::ErrorCode {
+        const std::string raw_id_str(raw_id, strnlen(raw_id, raw_id_max_len));
+        std::string physical_id = "28-" + raw_id_str;
         auto sensor_id = this->temp_->Register(physical_id);
 
         if (!sensor_id.has_value()) {
@@ -129,21 +137,39 @@ int EnvServiceFc::Initialize(const std::map<ara::core::StringView, ara::core::St
         sensorIdsToPaths[sensor_id.value()] = std::make_pair(label, physical_id);
         return core::ErrorCode::kOk;
     };
-    if (register_sensor(eeprom_cfg.value().board_temp1_id, "board_1") !=
+    if (register_sensor(eeprom_cfg.value().board_temp1_id,
+                        sizeof(eeprom_cfg.value().board_temp1_id), "board_1") !=
                                             core::ErrorCode::kOk) return core::ErrorCode::kInitializeError;
-    if (register_sensor(eeprom_cfg.value().board_temp2_id, "board_2") !=
+    if (register_sensor(eeprom_cfg.value().board_temp2_id,
+                        sizeof(eeprom_cfg.value().board_temp2_id), "board_2") !=
                                             core::ErrorCode::kOk) return core::ErrorCode::kInitializeError;
-    if (register_sensor(eeprom_cfg.value().board_temp3_id, "board_3") !=
+    if (register_sensor(eeprom_cfg.value().board_temp3_id,
+                        sizeof(eeprom_cfg.value().board_temp3_id), "board_3") !=
                                             core::ErrorCode::kOk) return core::ErrorCode::kInitializeError;
 
-    service_ipc.StartOffer();
-    service_udp.StartOffer();
+    auto i2c_imu = std::make_unique<i2c::I2CController>();
+    i2c::config_t imu_cfg;
+    imu_cfg.accel_scale = i2c::ACCEL_FULL_SCALE::k16g;
+    imu_cfg.accel_speed = i2c::ACCEL_SPEED_t::k208;
+    imu_cfg.gyro_scale = i2c::GYRO_FULL_SCALE::k2000;
+    imu_cfg.gyro_speed = i2c::GYRO_SPEED_t::k208;
+
+    ara::log::LogInfo() << "EnvApp: starting IMU initialization";
+    if (imu_.Initialize(std::move(i2c_imu), imu_cfg) == core::ErrorCode::kOk) {
+        imu_ready_ = true;
+        ara::log::LogInfo() << "EnvApp: IMU initialization complete";
+    } else {
+        imu_ready_ = false;
+        ara::log::LogWarn() << "EnvApp: IMU initialization failed, continuing without IMU";
+    }
+
     return core::ErrorCode::kOk;
 }
 
 int EnvServiceFc::LoadTempConfig(const std::map<ara::core::StringView, ara::core::StringView>& parms) {
     ara::log::LogDebug() << "Starting function LoadTempConfig";
-    const std::string path = parms.at("app_path") + "etc/config.json";
+    const auto app_path = parms.at("app_path");
+    const std::string path = std::string(app_path.data(), app_path.size()) + "etc/config.json";
     auto parser_opt = core::json::JsonParser::Parser(path);
     ara::log::LogDebug() << path;
     if (!parser_opt.has_value()) {
@@ -184,8 +210,33 @@ int EnvServiceFc::LoadTempConfig(const std::map<ara::core::StringView, ara::core
 
 int EnvServiceFc::Run(const std::stop_token& token) {
     ara::log::LogDebug() << "Leci run";
+
+    service_ipc.StartOffer();
+    service_udp.StartOffer();
     temp_->StartRxThread();
 
+    if (imu_ready_) {
+        imu_thread = std::jthread([this](const std::stop_token& imu_token) {
+            while (!imu_token.stop_requested()) {
+                core::condition::wait_for(std::chrono::milliseconds(10), imu_token);
+                const auto accel = imu_.ReadAccelData();
+                const auto gyro = imu_.ReadGyroData();
+                if (!accel.has_value() || !gyro.has_value()) {
+                    continue;
+                }
+                srp::env::IMUDataStructure data;
+                data.accel_x = accel.value().x;
+                data.accel_y = accel.value().y;
+                data.accel_z = accel.value().z;
+                data.gyroscope_x = gyro.value().x;
+                data.gyroscope_y = gyro.value().y;
+                data.gyroscope_z = gyro.value().z;
+                service_ipc.newIMUEvent.Update(data);
+                service_udp.newIMUEvent.Update(data);
+            }
+        });
+    }
+
     while (!token.stop_requested()) {
         const auto start = std::chrono::high_resolution_clock::now();
         const auto pressValue = this->bme->getPressure();
@@ -218,7 +269,12 @@ void EnvServiceFc::TempRxCallback(const std::vector<srp::mw::temp::TempReadHdr>&
         udpEv.Update(val);
     };
     for (auto &hdr : data) {
-        const auto& sensorName = sensorIdsToPaths[hdr.actuator_id].first;
+        const auto path_it = sensorIdsToPaths.find(hdr.actuator_id);
+        if (path_it == sensorIdsToPaths.end()) {
+            ara::log::LogWarn() << "Unknown sensor id: " << hdr.actuator_id;
+            continue;
+        }
+        const auto& sensorName = path_it->second.first;
         const int16_t value = static_cast<int16_t>(hdr.value * 10);
 
         ara::log::LogDebug() << "Receive temp id: " << hdr.actuator_id
diff --git a/apps/fc/env_service/env_service.hpp b/apps/fc/env_service/env_service.hpp
index 38b03af6..e1fa1b0b 100644
--- a/apps/fc/env_service/env_service.hpp
+++ b/apps/fc/env_service/env_service.hpp
@@ -24,6 +24,7 @@
 #include "mw/i2c_service/controller/bme280/controller.hpp"
 #include "srp/env/EnvAppFcSkeleton.h"
 #include "mw/i2c_service/controller/24lc32at/eeprom_config/cfg_manager.hpp"
+#include "mw/i2c_service/controller/LSM6DSOX/controller.hpp"
 
 namespace srp {
 namespace envServiceFc {
@@ -42,6 +43,11 @@ class EnvServiceFc final : public ara::exec::AdaptiveApplication {
 
   std::jthread bme_thread;
 
+  i2c::LSM6DSOX imu_;
+  bool imu_ready_{false};
+
+  std::jthread imu_thread;
+
   int LoadTempConfig(
     const std::map<ara::core::StringView, ara::core::StringView>& parms);
   void TempRxCallback(const std::vector<srp::mw::temp::TempReadHdr>& data);
diff --git a/apps/fc/logger_service/data_type.cpp b/apps/fc/logger_service/data_type.cpp
index 3f347db1..01fcfcf8 100644
--- a/apps/fc/logger_service/data_type.cpp
+++ b/apps/fc/logger_service/data_type.cpp
@@ -20,7 +20,8 @@ namespace {
   static constexpr auto kCsv_separator = ";";
   static constexpr auto kCsvHeader =
       "TIMESTAMP;BOARD_TEMP1;BOARD_TEMP2;BOARD_TEMP3;BME_TEMP;BME_HUMIDITY;"
-      "BME_ALTITUDE;CPU_USAGE;MEM_USAGE;DISK_UTILIZATION;APOGEE_DETECTED;MAIN_PARACHUTE_DETECTED";
+      "BME_ALTITUDE;CPU_USAGE;MEM_USAGE;DISK_UTILIZATION;APOGEE_DETECTED;MAIN_PARACHUTE_DETECTED;"
+      "FC_MODE;GPS_LAT;GPS_LON;GPS_ALT;GYRO_X;GYRO_Y;GYRO_Z;ACCEL_X;ACCEL_Y;ACCEL_Z";
 }
 
 std::string Data_t::get_header() {
@@ -39,8 +40,21 @@ std::string Data_t::to_string(const std::string& timestamp) {
   const auto cpu  = sys_cpu_usage_.load(std::memory_order_relaxed);
   const auto mem  = sys_mem_usage_.load(std::memory_order_relaxed);
   const auto disk = sys_disk_utilization_.load(std::memory_order_relaxed);
-  const auto apogee  = apogee_detected_.load(std::memory_order_relaxed);
-  const auto mainParachute = main_parachute_detected_.load(std::memory_order_relaxed);
+  const auto apogee  = static_cast<int>(apogee_detected_.load(std::memory_order_relaxed));
+  const auto mainParachute = static_cast<int>(main_parachute_detected_.load(std::memory_order_relaxed));
+  const auto fc_mode = static_cast<int>(FC_mode.load(std::memory_order_relaxed));
+
+  const auto lat = lat_.load(std::memory_order_relaxed);
+  const auto lon = lon_.load(std::memory_order_relaxed);
+  const auto alt = alt_.load(std::memory_order_relaxed);
+
+  const auto gyrox = gyro_x_.load(std::memory_order_relaxed);
+  const auto gyroy = gyro_y_.load(std::memory_order_relaxed);
+  const auto gyroz = gyro_z_.load(std::memory_order_relaxed);
+
+  const auto accelx = accel_x_.load(std::memory_order_relaxed);
+  const auto accely = accel_y_.load(std::memory_order_relaxed);
+  const auto accelz = accel_z_.load(std::memory_order_relaxed);
 
   std::stringstream res;
   res << std::fixed << std::setprecision(2);
@@ -56,9 +70,38 @@ std::string Data_t::to_string(const std::string& timestamp) {
   res << disk << kCsv_separator;
   res << apogee << kCsv_separator;
   res << mainParachute << kCsv_separator;
+  res << fc_mode << kCsv_separator;
+  res << lat << kCsv_separator;
+  res << lon << kCsv_separator;
+  res << alt << kCsv_separator;
+  res << gyrox << kCsv_separator;
+  res << gyroy << kCsv_separator;
+  res << gyroz << kCsv_separator;
+  res << accelx << kCsv_separator;
+  res << accely << kCsv_separator;
+  res << accelz;
   return res.str();
 }
 
+void Data_t::SetIMU(float ax, float ay, float az, float gx, float gy, float gz) {
+  accel_x_.store(ax, std::memory_order_relaxed);
+  accel_y_.store(ay, std::memory_order_relaxed);
+  accel_z_.store(az, std::memory_order_relaxed);
+  gyro_x_.store(gx, std::memory_order_relaxed);
+  gyro_y_.store(gy, std::memory_order_relaxed);
+  gyro_z_.store(gz, std::memory_order_relaxed);
+}
+
+void Data_t::SetGpsData(const float lat, const float lot, const float alt) {
+  lat_.store(lat, std::memory_order_relaxed);
+  lon_.store(lot, std::memory_order_relaxed);
+  alt_.store(alt, std::memory_order_relaxed);
+}
+
+void Data_t::SetFCMode(const uint8_t mode) {
+  FC_mode.store(mode, std::memory_order_relaxed);
+}
+
 void Data_t::SetBoardTemp1(tempType temp) {
   board_temp1_.store(temp, std::memory_order_relaxed);
 }
diff --git a/apps/fc/logger_service/data_type.hpp b/apps/fc/logger_service/data_type.hpp
index 5ecef751..2caadaf3 100644
--- a/apps/fc/logger_service/data_type.hpp
+++ b/apps/fc/logger_service/data_type.hpp
@@ -37,7 +37,27 @@ class Data_t {
   std::atomic<bool> apogee_detected_{false};
   std::atomic<bool> main_parachute_detected_{false};
 
+  // IMU
+  std::atomic<float> gyro_x_{0.0f};
+  std::atomic<float> gyro_y_{0.0f};
+  std::atomic<float> gyro_z_{0.0f};
+
+  std::atomic<float> accel_x_{0.0f};
+  std::atomic<float> accel_y_{0.0f};
+  std::atomic<float> accel_z_{0.0f};
+
+  std::atomic<float> lat_{0.0f};
+  std::atomic<float> lon_{0.0f};
+  std::atomic<float> alt_{0.0f};
+
+  // RADIO APP
+
+  std::atomic<uint8_t> FC_mode{0};
+
  public:
+  void SetIMU(float ax, float ay, float az, float gx, float gy, float gz);
+  void SetGpsData(const float lat, const float lot, const float alt);
+  void SetFCMode(const uint8_t mode);
   std::string get_header();
   std::string to_string(const std::string& timestamp);
   void SetBoardTemp1(tempType temp);
diff --git a/apps/fc/logger_service/service/logger_service.cpp b/apps/fc/logger_service/service/logger_service.cpp
index 2c1a2f57..fbd47f4f 100644
--- a/apps/fc/logger_service/service/logger_service.cpp
+++ b/apps/fc/logger_service/service/logger_service.cpp
@@ -25,9 +25,11 @@ namespace logger {
 namespace {
   static constexpr auto kLoggerFilename = "_fc_log.csv";
   static constexpr auto kLoggerFilenamePrefix = "/home/root/";
-  static constexpr std::uint16_t kSaveIntervalMs = 5000;
+  static constexpr std::uint16_t kSaveIntervalMs = 10;
   static constexpr auto kEnvServicePathName = "srp/apps/FcFileLoggerApp/envServiceFc_ipc";
   static constexpr auto kApogeeServocePathName = "srp/apps/ApogeeDetectService/ApogeeDetectService";
+  static constexpr auto kMainServicePathName = "srp/apps/MainService/MainService";
+  static constexpr auto kGpsServicePathName = "srp/apps/GPSService/GPSService";
   static constexpr auto kUdpServicePathName = "srp/apps/FcFileLoggerApp/logService_udp";
   static constexpr auto kIpcServicePathName = "srp/apps/FcFileLoggerApp/logService_ipc";
   static constexpr auto kSysStatServicePathName = "srp/apps/FcFileLoggerApp/FcSysStatService_ipc";
@@ -40,8 +42,12 @@ LoggerService::LoggerService()
     : env_service_proxy_{ara::core::InstanceSpecifier{kEnvServicePathName}},
       stat_service_proxy_{ara::core::InstanceSpecifier{kSysStatServicePathName}},
       apogee_proxy_{ara::core::InstanceSpecifier{kApogeeServocePathName}},
+      main_proxy_{ara::core::InstanceSpecifier{kMainServicePathName}},
+      gps_proxy_{ara::core::InstanceSpecifier{kGpsServicePathName}},
+      gps_handler_{nullptr},
       env_service_handler_{nullptr},
       stat_service_handler_{nullptr},
+      main_handler_{nullptr},
       apogee_handler_{nullptr},
       did_instance_{kFileDidPathName},
       timestamp_{std::make_shared<core::timestamp::TimestampController>()},
@@ -132,6 +138,31 @@ void LoggerService::SaveLoop(const std::stop_token& token) {
 }
 
 void LoggerService::SomeIpInit() {
+  gps_proxy_.StartFindService([this](auto handler) {
+    gps_handler_ = handler;
+    gps_handler_->GPSStatusEvent.Subscribe(1, [this](std::uint8_t status) {
+      gps_handler_->GPSStatusEvent.SetReceiveHandler([this]() {
+        const auto res_opt = gps_handler_->GPSStatusEvent.GetNewSamples();
+        if (!res_opt.HasValue()) {
+          return;
+        }
+        const auto res = res_opt.Value();
+        data_.SetGpsData(res.latitude, res.longitude, res.altitude);
+      });
+    });
+  });
+  main_proxy_.StartFindService([this](auto handler) {
+    main_handler_ = handler;
+    main_handler_->CurrentModeStatusEvent.Subscribe(1, [this](std::uint8_t statuc) {
+      main_handler_->CurrentModeStatusEvent.SetReceiveHandler([this]() {
+        const auto res_opt = main_handler_->CurrentModeStatusEvent.GetNewSamples();
+        if (!res_opt.HasValue()) {
+          return;
+        }
+        data_.SetFCMode(res_opt.Value());
+      });
+    });
+  });
   apogee_proxy_.StartFindService([this](auto handler) {
     apogee_handler_ = handler;
     apogee_handler_->newApogeeDetected.Subscribe(1, [this](std::uint8_t status) {
@@ -170,6 +201,17 @@ void LoggerService::SomeIpInit() {
   env_service_proxy_.StartFindService([this](auto handler) {
     env_service_handler_ = handler;
 
+    env_service_handler_->newIMUEvent.Subscribe(1, [this](std::uint8_t status) {
+      env_service_handler_->newIMUEvent.SetReceiveHandler([this]() {
+        const auto res_opt = env_service_handler_->newIMUEvent.GetNewSamples();
+        if (!res_opt.HasValue()) {
+          return;
+        }
+        const auto res = res_opt.Value();
+        data_.SetIMU(res.accel_x, res.accel_y, res.accel_z,
+                                        res.gyroscope_x, res.gyroscope_y, res.gyroscope_z);
+      });
+    });
     env_service_handler_->newBoardTempEvent_1.Subscribe(1, [this](std::uint8_t /*status*/) {
       env_service_handler_->newBoardTempEvent_1.SetReceiveHandler([this]() {
         auto res = env_service_handler_->newBoardTempEvent_1.GetNewSamples();
diff --git a/apps/fc/logger_service/service/logger_service.hpp b/apps/fc/logger_service/service/logger_service.hpp
index ef2f6c93..d179671d 100644
--- a/apps/fc/logger_service/service/logger_service.hpp
+++ b/apps/fc/logger_service/service/logger_service.hpp
@@ -23,13 +23,19 @@
 #include "core/timestamp/timestamp_driver.hpp"
 #include "srp/apps/FcSysStatService/FcSysStatServiceHandler.h"
 #include "srp/env/EnvAppFc/EnvAppFcHandler.h"
+#include "srp/apps/MainService/MainServiceHandler.h"
 #include "srp/apps/ApogeeDetectService/ApogeeDetectServiceHandler.h"
+#include "srp/apps/GPSService/GPSServiceHandler.h"
 
 namespace srp {
 namespace logger {
 
 class LoggerService final : public ara::exec::AdaptiveApplication {
  private:
+  std::shared_ptr<apps::GPSServiceHandler> gps_handler_;
+  apps::GPSServiceProxy gps_proxy_;
+  std::shared_ptr<apps::MainServiceHandler> main_handler_;
+  apps::MainServiceProxy main_proxy_;
   env::EnvAppFcProxy env_service_proxy_;
   apps::FcSysStatServiceProxy stat_service_proxy_;
   std::shared_ptr<env::EnvAppFcHandler> env_service_handler_;
diff --git a/apps/fc/system_stat_service/system_stat_service.cpp b/apps/fc/system_stat_service/system_stat_service.cpp
index d92ac988..b1b284b0 100644
--- a/apps/fc/system_stat_service/system_stat_service.cpp
+++ b/apps/fc/system_stat_service/system_stat_service.cpp
@@ -17,7 +17,6 @@
 #include "apps/fc/system_stat_service/system_stat_service.hpp"
 #include "core/common/condition.h"
 #include "ara/log/log.h"
-#include "core/sys/system_stat.hpp"
 
 namespace srp {
 namespace sysService {
@@ -41,20 +40,21 @@ int FcSystemStatService::Initialize(const std::map<ara::core::StringView, ara::c
 
 std::optional<apps::FcSysStatType> FcSystemStatService::GetSysStats() const {
     apps::FcSysStatType stats;
-    auto cpu_usage_opt = core::stat::SystemStats::get_cpu_usage();
+    auto cpu_usage_opt = stats_.get_cpu_usage();
     if (!cpu_usage_opt.has_value()) {
         return std::nullopt;
     }
     stats.cpu_usage = static_cast<float>(cpu_usage_opt.value());
-    auto mem_usage_opt = core::stat::SystemStats::get_ram_usage();
+    auto mem_usage_opt = stats_.get_ram_usage();
     if (!mem_usage_opt.has_value()) {
         return std::nullopt;
     }
     stats.mem_usage = mem_usage_opt.value();
-    stats.disk_utilization = static_cast<float>(core::stat::SystemStats::get_disk_space());
+    stats.disk_utilization = static_cast<float>(stats_.get_disk_space());
     return stats;
 }
 
+
 int FcSystemStatService::Run(const std::stop_token& token) {
     service_ipc.StartOffer();
     service_udp.StartOffer();
diff --git a/apps/fc/system_stat_service/system_stat_service.hpp b/apps/fc/system_stat_service/system_stat_service.hpp
index e36e229e..35a1ddf3 100644
--- a/apps/fc/system_stat_service/system_stat_service.hpp
+++ b/apps/fc/system_stat_service/system_stat_service.hpp
@@ -19,6 +19,7 @@
 #include <utility>
 #include "ara/exec/adaptive_application.h"
 #include "srp/apps/FcSysStatServiceSkeleton.h"
+#include "core/sys/system_stat.hpp"
 
 namespace srp {
 namespace sysService {
@@ -28,6 +29,8 @@ class FcSystemStatService final : public ara::exec::AdaptiveApplication {
   apps::FcSysStatServiceSkeleton service_ipc;
   apps::FcSysStatServiceSkeleton service_udp;
 
+  core::stat::SystemStats stats_;
+
   std::optional<apps::FcSysStatType> GetSysStats() const;
 
  protected:
diff --git a/communication-core/sockets/ipc_socket.cc b/communication-core/sockets/ipc_socket.cc
index 6d841a89..cd683462 100644
--- a/communication-core/sockets/ipc_socket.cc
+++ b/communication-core/sockets/ipc_socket.cc
@@ -116,10 +116,17 @@ void IpcSocket::Loop(std::stop_token stoken) {
 }
 
 void IpcSocket::StopRXThread() {
+  if (!this->rx_thred) {
+    return;
+  }
   this->rx_thred->request_stop();
   this->rx_thred->join();
+  this->rx_thred.reset();
 }
 IpcSocket::~IpcSocket() {
+  if (!this->rx_thred) {
+    return;
+  }
   this->rx_thred->request_stop();
   this->rx_thred->join();
 }
diff --git a/communication-core/sockets/stream_ipc_socket.cc b/communication-core/sockets/stream_ipc_socket.cc
index 4ace81fc..cdce03aa 100644
--- a/communication-core/sockets/stream_ipc_socket.cc
+++ b/communication-core/sockets/stream_ipc_socket.cc
@@ -55,17 +55,17 @@ std::optional<std::vector<uint8_t>> StreamIpcSocket::Transmit(
   struct timeval timeout;
   timeout.tv_sec = 2;
   timeout.tv_usec = 0;
-  setsockopt(server_sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
+  setsockopt(server_socket, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));
   struct sockaddr_un server_addr;
   server_addr.sun_family = AF_UNIX;
   strcpy(server_addr.sun_path, ("/run/" + ip).c_str());  // NOLINT
   if (connect(server_socket, (struct sockaddr*)&server_addr,
               sizeof(server_addr)) == -1) {
-    close(server_sock);
+    close(server_socket);
     return std::nullopt;
   }
   if (write(server_socket, payload.data(), payload.size()) < 0) {
-    close(server_sock);
+    close(server_socket);
     return std::nullopt;
   }
   std::array<char, 256 * 2> buffer;
diff --git a/core/sys/system_stat.cpp b/core/sys/system_stat.cpp
index a5b64cc8..6fcae55d 100644
--- a/core/sys/system_stat.cpp
+++ b/core/sys/system_stat.cpp
@@ -19,15 +19,21 @@
 #include <string>
 #include <filesystem>
 #include <cmath>
+#include <thread>  // NOLINT
+#include <atomic>
+#include <chrono>
+#include <mutex>
+
 #include "ara/log/log.h"
 
+
 namespace srp {
 namespace core {
 namespace stat {
 
 namespace {
     static constexpr auto kMemInfoPath = "/proc/meminfo";
-    static constexpr auto kCPU_THREADS = 1;
+    static constexpr std::chrono::milliseconds kCpuSampleInterval{100};
 }
 
 namespace fs = std::filesystem;
@@ -56,12 +62,85 @@ std::optional<float> SystemStats::get_ram_usage() {
     return 100.0f * static_cast<float>(total - available) / static_cast<float>(total);
 }
 
+struct CPUState {
+    unsigned long long user, nice, system, idle, iowait, irq, softirq, steal;
+};
+
+// Funkcja pomocnicza do czytania /proc/stat
+std::optional<CPUState> read_cpu_state() {
+    std::ifstream file("/proc/stat");
+    if (!file.is_open()) return std::nullopt;
+
+    std::string cpu;
+    CPUState state;
+    // Interesuje nas pierwsza linijka podsumowująca cały procesor
+    if (file >> cpu >> state.user >> state.nice >> state.system >> state.idle 
+             >> state.iowait >> state.irq >> state.softirq >> state.steal) {
+        return state;
+    }
+    return std::nullopt;
+}
+
+namespace {
+std::atomic<bool> g_cpu_sampler_started{false};
+std::atomic<bool> g_has_cpu_sample{false};
+std::atomic<double> g_last_cpu_usage{0.0};
+std::once_flag g_cpu_sampler_once;
+
+double calculate_cpu_usage(const CPUState& prev, const CPUState& curr) {
+    auto prev_idle = prev.idle + prev.iowait;
+    auto curr_idle = curr.idle + curr.iowait;
+
+    auto prev_total =
+        prev.user + prev.nice + prev.system + prev.idle + prev.iowait + prev.irq + prev.softirq +
+        prev.steal;
+    auto curr_total =
+        curr.user + curr.nice + curr.system + curr.idle + curr.iowait + curr.irq + curr.softirq +
+        curr.steal;
+
+    auto total_delta = curr_total - prev_total;
+    auto idle_delta = curr_idle - prev_idle;
+    if (total_delta == 0) {
+        return 0.0;
+    }
+
+    return 100.0 * static_cast<double>(total_delta - idle_delta) / static_cast<double>(total_delta);
+}
+
+void start_cpu_sampler_thread() {
+    std::call_once(g_cpu_sampler_once, [] {
+        std::thread([] {
+            auto prev = read_cpu_state();
+            if (!prev) {
+                return;
+            }
+
+            g_cpu_sampler_started.store(true, std::memory_order_release);
+
+            while (true) {
+                std::this_thread::sleep_for(kCpuSampleInterval);
+                auto curr = read_cpu_state();
+                if (!curr) {
+                    continue;
+                }
+
+                g_last_cpu_usage.store(calculate_cpu_usage(*prev, *curr), std::memory_order_release);
+                g_has_cpu_sample.store(true, std::memory_order_release);
+                prev = curr;
+            }
+        }).detach();
+    });
+}
+}  // namespace
+
 std::optional<double> SystemStats::get_cpu_usage() {
-    double load[1];
-    if (getloadavg(load, 1) <= 0) return std::nullopt;
+    start_cpu_sampler_thread();
+    if (!g_cpu_sampler_started.load(std::memory_order_acquire) ||
+        !g_has_cpu_sample.load(std::memory_order_acquire)) {
+        return std::nullopt;
+    }
 
-    double usage = load[0] * 100.0 / kCPU_THREADS;
-    return (usage > 100.0) ? 100.0 : usage;
+    return g_last_cpu_usage.load(std::memory_order_acquire);
 }
 
 }  // namespace stat
diff --git a/deployment/apps/env_service/config.json b/deployment/apps/env_service/config.json
index a8d0fa9d..d0619520 100644
--- a/deployment/apps/env_service/config.json
+++ b/deployment/apps/env_service/config.json
@@ -21,7 +21,7 @@
   ],
   "sensors-temp": [
     {
-      "physical_id" : "28-0b2554e84c21",
+      "physical_id" : "28-0b2554dfd2cc",
       "name" : "sensor_temp_1"
     },
     {
diff --git a/deployment/apps/fc/logger_service/BUILD b/deployment/apps/fc/logger_service/BUILD
index 0d7e9c15..b9854953 100644
--- a/deployment/apps/fc/logger_service/BUILD
+++ b/deployment/apps/fc/logger_service/BUILD
@@ -21,6 +21,8 @@ filegroup(
         "//deployment/apps/fc/system_stat_service:instance",
         "//deployment/system_definition/diag/jobs/file_logger:file_logger_job",
         "//deployment/apps/fc/apogee_detect_service:instance",
+        "//deployment/system_definition/someip/fc/gps_service:service_someipy",
+        "//deployment/system_definition/someip/fc/main_service:service_someipy"
     ],
     visibility = ["//visibility:public"],
 )
diff --git a/deployment/apps/fc/logger_service/app_config.json b/deployment/apps/fc/logger_service/app_config.json
index 6b17d6ef..7ed399fc 100644
--- a/deployment/apps/fc/logger_service/app_config.json
+++ b/deployment/apps/fc/logger_service/app_config.json
@@ -3,7 +3,9 @@
         "deployment/system_definition/someip/fc/logger_service/service.json",
         "deployment/system_definition/someip/fc/env_service/service.json",
         "deployment/system_definition/someip/fc/sys_stat_service/service.json",
-        "deployment/system_definition/someip/fc/apogee_service/service.json"
+        "deployment/system_definition/someip/fc/apogee_service/service.json",
+        "deployment/system_definition/someip/fc/gps_service/service.json",
+        "deployment/system_definition/someip/fc/main_service/service.json"
     ],
     "package": "srp.apps",
     "adaptive_application": {
@@ -71,6 +73,16 @@
                     "name": "ApogeeDetectService as ApogeeDetectService",
                     "on": "ipc",
                     "instance": 2
+                },
+                {
+                    "name": "MainService as MainService",
+                    "on": "ipc",
+                    "instance": 2
+                },
+                {
+                    "name": "GPSService as GPSService",
+                    "on": "ipc",
+                    "instance": 2
                 }
             ]
         }
diff --git a/deployment/apps/servo_service/app_config.json b/deployment/apps/servo_service/app_config.json
index 4d0d8dc7..83909c5f 100644
--- a/deployment/apps/servo_service/app_config.json
+++ b/deployment/apps/servo_service/app_config.json
@@ -18,7 +18,7 @@
                 "logger": {
                     "app_id": "SERV",
                     "app_des": "Serwis od servo",
-                    "log_level": "kWarn",
+                    "log_level": "kDebug",
                     "log_mode": "kRemote",
                     "ctx": [
                         {
@@ -43,12 +43,12 @@
                         },
                         {
                             "ctx_id": "srdr",
-                            "log_level": "kWarn",
+                            "log_level": "kDebug",
                             "ctx_des": "Servo Driver Logger"
                         },
                         {
                             "ctx_id": "srct",
-                            "log_level": "kWarn",
+                            "log_level": "kDebug",
                             "ctx_des": "Servo Controller Logger"
                         }
                     ]
diff --git a/deployment/cpu/fc/BUILD b/deployment/cpu/fc/BUILD
index 2777b29b..43acf207 100644
--- a/deployment/cpu/fc/BUILD
+++ b/deployment/cpu/fc/BUILD
@@ -22,8 +22,8 @@ cpu_def(
 
         # "//deployment/example_adaptive/ExampleApp",
         "//deployment/apps/fc/env_service:EnvAppFc",
-        "//deployment/apps/fc/gps_app:GPSApp",
-        "//deployment/apps/fc/recovery_service:RecoveryService",
+        # "//deployment/apps/fc/gps_app:GPSApp",
+        # "//deployment/apps/fc/recovery_service:RecoveryService",
         "//deployment/apps/fc/radio_app:RadioApp",
         "//deployment/apps/fc/main_app:MainApp",
         "//deployment/apps/fc/system_stat_service:FcSysStatService",
diff --git a/deployment/tools/ecu/BUILD b/deployment/tools/ecu/BUILD
index 23002375..dddb1984 100644
--- a/deployment/tools/ecu/BUILD
+++ b/deployment/tools/ecu/BUILD
@@ -3,7 +3,7 @@ load("@srp_platform//tools/common:flash_ecu.bzl", "flash_ecu")
 flash_ecu(
     name = "flash_ec",
     file = ["//deployment/cpu/ec:pkg"],
-    ip = "192.168.10.101",
+    ip = "192.168.10.51",
     tool = select({
         "//:sim": "//deployment/bazel:docker_flash_sh",
         "//conditions:default": "//deployment/bazel:flash_sh",
@@ -14,7 +14,7 @@ flash_ecu(
 flash_ecu(
     name = "flash_fc",
     file = ["//deployment/cpu/fc:pkg"],
-    ip = "192.168.10.101",
+    ip = "192.168.10.52",
     tool = select({
         "//:sim": "//deployment/bazel:docker_flash_sh",
         "//conditions:default": "//deployment/bazel:flash_sh",
diff --git a/mw/i2c_service/controller/24lc32at/eeprom_config/cfg_manager.hpp b/mw/i2c_service/controller/24lc32at/eeprom_config/cfg_manager.hpp
index 2cc9ed6c..ec227d4d 100644
--- a/mw/i2c_service/controller/24lc32at/eeprom_config/cfg_manager.hpp
+++ b/mw/i2c_service/controller/24lc32at/eeprom_config/cfg_manager.hpp
@@ -23,7 +23,7 @@ namespace srp {
 namespace eeprom {
 
 struct [[gnu::packed]] EEPROM_config {
-    float pca9685_XO_corelation;
+    float pca9685_XO_corelation{1.0f};
     char board_temp1_id[13];
     char board_temp2_id[13];
     char board_temp3_id[13];
diff --git a/mw/i2c_service/controller/LSM6DSOX/consts.hpp b/mw/i2c_service/controller/LSM6DSOX/consts.hpp
index 14471dfa..a7867656 100644
--- a/mw/i2c_service/controller/LSM6DSOX/consts.hpp
+++ b/mw/i2c_service/controller/LSM6DSOX/consts.hpp
@@ -14,7 +14,7 @@
 namespace srp {
 namespace i2c {
 enum class ACCEL_SPEED_t: uint8_t {
-    k12_5 = 0b10110000,
+    k12_5 = 0b00010000,
     k26 =   0b00100000,
     k52 =   0b00110000,
     k104 =  0b01000000,
diff --git a/mw/i2c_service/controller/LSM6DSOX/controller.cpp b/mw/i2c_service/controller/LSM6DSOX/controller.cpp
index db84410a..7f8d2c7d 100644
--- a/mw/i2c_service/controller/LSM6DSOX/controller.cpp
+++ b/mw/i2c_service/controller/LSM6DSOX/controller.cpp
@@ -14,14 +14,26 @@
 namespace srp {
 namespace i2c {
 namespace {
-    static constexpr uint8_t kCTRL2_G = 0x11;
     static constexpr uint8_t kOUTX_L_G = 0x22;
     static constexpr uint8_t kOUTX_L_XL = 0x28;
     static constexpr uint8_t kI2C_ADDR = 0x6A;
     static constexpr uint8_t kACCEL_SET_REG = 0x10;  // CTRL1_XL
     static constexpr uint8_t kGYRO_SET_REG = 0x11;  // CTRL2_G
-    static constexpr uint8_t kSTATUS_REG = 0x1E;  // STATUS_REG
-}
+    static constexpr uint8_t kSTATUS_REG = 0x1E;
+    static constexpr uint8_t kWHO_AM_I_REG = 0x0F;
+    static constexpr uint8_t kWHO_AM_I_EXPECTED = 0x6C;
+    static constexpr uint8_t kCTRL3_C = 0x12;
+    // BDU (bit 6) + IF_INC (bit 2); bit 1 must stay 0 (DS12140 Table 49).
+    static constexpr uint8_t kCTRL3_C_BDU_IF_INC = 0x44;
+    static constexpr uint8_t kCTRL9_XL = 0x18;
+    // DEN_X/Y/Z = 1, I3C_disable = 1, reserved LSB = 0 (DS12140 Table 66–67).
+    static constexpr uint8_t kCTRL9_XL_I2C_ONLY = 0xE2;
+
+    int16_t BytesToInt16(uint8_t lo, uint8_t hi) {
+        return static_cast<int16_t>(
+            static_cast<uint16_t>(lo) | (static_cast<uint16_t>(hi) << 8U));
+    }
+}  // namespace
 
 core::ErrorCode LSM6DSOX::InitializeGyro(const config_t& config) {
     gyroSensitivity = GetGyroSensitivity(config.gyro_scale);
@@ -40,59 +52,81 @@ core::ErrorCode LSM6DSOX::Initialize(std::unique_ptr<II2CController> i2c, const
     if (i2c_->Init(std::make_unique<com::soc::StreamIpcSocket>()) != core::ErrorCode::kOk) {
         return core::ErrorCode::kInitializeError;
     }
+    const auto who = i2c_->Read(kI2C_ADDR, kWHO_AM_I_REG, 1);
+    if (!who.has_value() || who->empty() || (*who)[0] != kWHO_AM_I_EXPECTED) {
+        ara::log::LogError() << "LSM6DSO WHO_AM_I read failed or unexpected value";
+        return core::ErrorCode::kInitializeError;
+    }
+    if (i2c_->Write(kI2C_ADDR, {kCTRL9_XL, kCTRL9_XL_I2C_ONLY}) != core::ErrorCode::kOk) {
+        ara::log::LogError() << "Failed to configure CTRL9_XL (I3C disable)";
+        return core::ErrorCode::kInitializeError;
+    }
+    if (i2c_->Write(kI2C_ADDR, {kCTRL3_C, kCTRL3_C_BDU_IF_INC}) != core::ErrorCode::kOk) {
+        ara::log::LogError() << "Failed to configure CTRL3_C (BDU, IF_INC)";
+        return core::ErrorCode::kInitializeError;
+    }
     auto err = core::ErrorCode::kOk;
     if (InitializeGyro(config) != core::ErrorCode::kOk) {
-        ara::log::LogError() << "Failed to initiaize Gyroscope";
+        ara::log::LogError() << "Failed to initialize Gyroscope";
         err = core::ErrorCode::kInitializeError;
     }
     if (InitializeAccel(config) != core::ErrorCode::kOk) {
-        ara::log::LogError() << "Failed to initiaize Accelerometr";
+        ara::log::LogError() << "Failed to initialize Accelerometer";
         err = core::ErrorCode::kInitializeError;
     }
     return err;
 }
 std::optional<SensorData> LSM6DSOX::ReadGyroData() {
     const auto status = this->i2c_->Read(kI2C_ADDR, kSTATUS_REG, 1);
-    if (!status.has_value() || (status.value()[0] & 0x02) == 0) {
+    if (!status.has_value() || status->empty() || ((*status)[0] & 0x02) == 0) {
         return std::nullopt;
     }
     const auto buffer_opt = this->i2c_->Read(kI2C_ADDR, kOUTX_L_G, 6);
     if (!buffer_opt.has_value()) {
         return std::nullopt;
     }
-    const auto buffer = buffer_opt.value();
-    const int16_t x = buffer[1] << 8 | buffer[0];
-    const int16_t y = buffer[3] << 8 | buffer[2];
-    const int16_t z = buffer[5] << 8 | buffer[4];
+    const auto& buffer = buffer_opt.value();
+    if (buffer.size() < 6) {
+        return std::nullopt;
+    }
+    const int16_t x = BytesToInt16(buffer[0], buffer[1]);
+    const int16_t y = BytesToInt16(buffer[2], buffer[3]);
+    const int16_t z = BytesToInt16(buffer[4], buffer[5]);
+    // GetGyroSensitivity: hundredths of mdps/LSB (e.g. 875 -> 8.75 mdps/LSB, Table 2).
     const SensorData data = {
-        .x = x * gyroSensitivity / 100.0f,
-        .y = y * gyroSensitivity / 100.0f,
-        .z = z * gyroSensitivity / 100.0f
+        .x = x * gyroSensitivity / 100000.0f,
+        .y = y * gyroSensitivity / 100000.0f,
+        .z = z * gyroSensitivity / 100000.0f
     };
     return data;
 }
 std::optional<SensorData> LSM6DSOX::ReadAccelData() {
     const auto status = this->i2c_->Read(kI2C_ADDR, kSTATUS_REG, 1);
-    if (!status.has_value() || (status.value()[0] & 0x01) == 0) {
+    if (!status.has_value() || status->empty() || ((*status)[0] & 0x01) == 0) {
         return std::nullopt;
     }
     const auto buffer_opt = this->i2c_->Read(kI2C_ADDR, kOUTX_L_XL, 6);
     if (!buffer_opt.has_value()) {
         return std::nullopt;
     }
-    const auto buffer = buffer_opt.value();
-    const int16_t x = buffer[1] << 8 | buffer[0];
-    const int16_t y = buffer[3] << 8 | buffer[2];
-    const int16_t z = buffer[5] << 8 | buffer[4];
+    const auto& buffer = buffer_opt.value();
+    if (buffer.size() < 6) {
+        return std::nullopt;
+    }
+    const int16_t x = BytesToInt16(buffer[0], buffer[1]);
+    const int16_t y = BytesToInt16(buffer[2], buffer[3]);
+    const int16_t z = BytesToInt16(buffer[4], buffer[5]);
+    // GetAccelSensitivity: thousandths of mg/LSB (e.g. 61 -> 0.061 mg/LSB, Table 2).
     const SensorData data = {
-        .x = x * accelSensitivity / 1000.0f,
-        .y = y * accelSensitivity / 1000.0f,
-        .z = z * accelSensitivity / 1000.0f
+        .x = x * accelSensitivity / 1000000.0f,
+        .y = y * accelSensitivity / 1000000.0f,
+        .z = z * accelSensitivity / 1000000.0f
     };
     return data;
 }
 
 int32_t LSM6DSOX::GetAccelSensitivity(const ACCEL_FULL_SCALE scale) {
+    // LA_So from Table 2 [mg/LSB] * 1000 for integer storage (0.061 -> 61).
     switch (scale) {
         case ACCEL_FULL_SCALE::k2g : return 61;
         case ACCEL_FULL_SCALE::k4g : return 122;
@@ -103,6 +137,7 @@ int32_t LSM6DSOX::GetAccelSensitivity(const ACCEL_FULL_SCALE scale) {
 }
 
 int32_t LSM6DSOX::GetGyroSensitivity(const GYRO_FULL_SCALE scale) {
+    // G_So from Table 2 [mdps/LSB] * 100 (8.75 -> 875). ±125 dps: 4.375 mdps/LSB.
     switch (scale) {
         case GYRO_FULL_SCALE::k250 : return 875;
         case GYRO_FULL_SCALE::k500 : return 1750;
diff --git a/mw/i2c_service/controller/LSM6DSOX/lsm6dso.pdf b/mw/i2c_service/controller/LSM6DSOX/lsm6dso.pdf
new file mode 100644
index 00000000..8636749b
Binary files /dev/null and b/mw/i2c_service/controller/LSM6DSOX/lsm6dso.pdf differ
diff --git a/mw/temp/controller/temp_controller.cpp b/mw/temp/controller/temp_controller.cpp
index 9c0b356d..d25604c5 100644
--- a/mw/temp/controller/temp_controller.cpp
+++ b/mw/temp/controller/temp_controller.cpp
@@ -33,7 +33,9 @@ srp::core::ErrorCode TempController::Init(uint16_t service_id, std::unique_ptr<c
         return core::ErrorCode::kInitializeError;
     }
     this->sub_sock_ = std::move(sock);
-    this->sock = std::move(std::make_unique<com::soc::IpcSocket>());
+    if (!this->sock) {
+        this->sock = std::make_unique<com::soc::IpcSocket>();
+    }
     if (!this->sock) {
         ara::log::LogError() << "TempController::Init failed: cannot allocate IPC socket";
         return core::ErrorCode::kInitializeError;
@@ -106,16 +108,15 @@ std::vector<srp::mw::temp::TempReadHdr> TempController::Conv(const std::vector<u
 }
 
 void TempController::SetTempRXCallback() {
-    srp::com::soc::RXCallbackStream lambdaCallback = [this](
-        const std::string& ip, const std::uint16_t& port,
-            const std::vector<std::uint8_t> data) {
+    srp::com::soc::RXCallback lambdaCallback = [this](
+        const std::string& /*ip*/, const std::uint16_t& /*port*/,
+        const std::vector<std::uint8_t>& data) {
         auto hdr = this->Conv(data);
         if (!this->callback_) {
             ara::log::LogWarn() << "Temp RX callback dropped: callback not set";
-            return std::vector<uint8_t>{};
+            return;
         }
         this->callback_(hdr);
-        return std::vector<uint8_t>{};
     };
     this->sock->SetRXCallback(lambdaCallback);
 }
diff --git a/srp-common/.github/workflows/gh_action.yml b/srp-common/.github/workflows/gh_action.yml
new file mode 100644
index 00000000..deb0aeb0
--- /dev/null
+++ b/srp-common/.github/workflows/gh_action.yml
@@ -0,0 +1,57 @@
+---
+name: Lint, build and test SRP-common
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+    branches:
+      - master
+  merge_group:
+  workflow_dispatch:
+jobs:
+  cpplint:
+    name: CppLint Check
+    runs-on: ubuntu-latest
+    container:
+      image: twoja-nazwa-uzytkownika/twoj-obraz:tag
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: Run cpplint
+        run: |
+          /bin/bash ./tools/cpplint/cpplint.sh
+
+  bazel-test:
+    name: SRP Unit Tests
+    needs: cpplint
+    runs-on: self-hosted
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: Run Bazel tests
+        run: |
+          ./tools/ut/unittest.sh --no_cache
+
+  build-all:
+    name: Build All
+    needs:
+      - bazel-test
+    runs-on: self-hosted
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: Build All
+        run: |
+          ./tools/build_all/build_all.sh --no_cache
\ No newline at end of file