Fix lsm6dso driver

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;
 }
 

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():

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,55 +21,37 @@ 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() {
   return kCsv_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,106 +95,149 @@ 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);
   append_bytes(offset, servo_dump_status_local);
   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