Add apogee app

apps/fc/apogee_service/BUILD

@@ -0,0 +1,34 @@
+load("@srp_platform//tools/model_generator/ara:adaptive_application.bzl", "adaptive_application", "ara_runtime_lib", "ara_someip_lib")
+ara_runtime_lib(
+    name = "ara",
+    model_src = ["//deployment/apps/fc/apogee_detect_service:instance"],
+    visibility = ["//apps/fc/apogee_service:__subpackages__"],
+)
+
+ara_someip_lib(
+    name = "someip_lib",
+    model_src = ["//deployment/apps/fc/apogee_detect_service:instance"],
+    visibility = ["//apps/fc/apogee_service:__subpackages__"],
+)
+
+cc_binary(
+    name = "apogee_service_fc",
+    srcs = [
+        "main.cc",
+        "apogee_app.cpp",
+        "apogee_app.hpp",
+    ],
+    linkopts = ["-lm"],
+    visibility = [
+        "//deployment/apps:__subpackages__"
+    ],
+    deps = [
+        "@srp_platform//ara/exec:adaptive_application_lib",
+        "//core/common:condition_lib",
+
+        ":ara",
+        ":someip_lib",
+
+        "//core/apogee:apogee_lib", 
+    ],
+)

apps/fc/apogee_service/apogee_app.cpp

@@ -0,0 +1,124 @@
+/**
+ * @file apogee_app.cpp
+ * @author Wiktor Laska
+ * @version 0.1
+ * @date 2026-04-25
+ * @copyright Copyright (c) 2026
+ */
+#include "apogee_app.hpp"
+
+#include <cmath>
+#include <thread>  // NOLINT
+
+#include "ara/log/log.h"
+#include "core/common/condition.h"
+
+namespace srp {
+namespace apps {
+
+namespace {
+  static constexpr auto kEnv_service_path = "srp/apps/ApogeeDetectApp/EnvAppFc";
+  static constexpr auto kService_ipc_instance = "srp/apps/ApogeeDetectApp/ApogeeDetectService_ipc";
+  static constexpr auto kService_udp_instance = "srp/apps/ApogeeDetectApp/ApogeeDetectService_udp";
+  static constexpr auto kAlgorithm_delay_ms = 100;
+  static constexpr auto kMain_parachute_opening_apogee_m = 400;
+  static constexpr double kBasePressure = 1013.25;
+  static constexpr double kEncodedBarToHpa = 10.0;
+}
+
+ApogeeService::ApogeeService() :
+    env_service_proxy{ara::core::InstanceSpecifier{kEnv_service_path}},
+    env_service_handler{nullptr},
+    service_ipc{ara::core::InstanceSpecifier{kService_ipc_instance}},
+    service_udp{ara::core::InstanceSpecifier{kService_udp_instance}} {
+}
+
+int ApogeeService::Initialize(const std::map<ara::core::StringView, ara::core::StringView> parms) {
+    ara::log::LogInfo() << "ApogeeService: Initializing...";
+
+    this->service_ipc.StartOffer();
+    this->service_udp.StartOffer();
+    this->SomeIpInit();
+
+    return 0;
+}
+
+void ApogeeService::SomeIpInit() {
+  this->env_service_proxy.StartFindService([this](auto handler) {
+    this->env_service_handler = handler;
+    this->env_service_handler->newBME280Event.Subscribe(1, [this](const uint8_t status) {
+      this->env_service_handler->newBME280Event.SetReceiveHandler([this] () {
+        auto res = this->env_service_handler->newBME280Event.GetNewSamples();
+        if (res.HasValue()) {
+            this->latest_height_.store(res.Value().altitude);
+        }
+      });
+    });
+    this->env_service_handler->newIMUEvent.Subscribe(1, [this](const uint8_t status) {
+      this->env_service_handler->newIMUEvent.SetReceiveHandler([this]() {
+        auto res = this->env_service_handler->newIMUEvent.GetNewSamples();
+        if (res.HasValue()) {
+          if (!first_imu_measurement_) {
+            first_imu_measurement_ = true;
+          }
+          // TODO(matikrajek42@gmail.com) check wchich axis is valid
+          this->latest_velocity_ = res.Value().accel_z;
+        }
+      });
+    });
+  });
+}
+
+void ApogeeService::EvaluateApogee() {
+    if (!first_imu_measurement_) {
+      return;
+    }
+    if (is_apogee_detected) {
+      if (this->latest_height_ <= kMain_parachute_opening_apogee_m) {
+        is_main_parachute_detected = true;
+        this->service_ipc.newMainParachuteDetected.Update(true);
+        this->service_udp.newMainParachuteDetected.Update(true);
+      }
+      return;
+    }
+    const double current_height = this->latest_height_.load();
+    const double current_velocity = this->latest_velocity_.load();
+
+    apogee_detector_.update(current_height, current_velocity);
+
+    if (apogee_detector_.isApogeeReached()) {
+      is_apogee_detected = true;
+      ara::log::LogInfo() << "APOGEUM WYKRYT: " << "Max Wysokosc: "
+          << static_cast<float>(apogee_detector_.getApogee()) << " m, "
+          << "Ostatnia Predkosc: " << static_cast<float>(apogee_detector_.averageSpeed()) << " m/s";
+      this->service_ipc.newApogeeDetected.Update(true);
+      this->service_udp.newApogeeDetected.Update(true);
+    }
+}
+
+int ApogeeService::Run(const std::stop_token& token) {
+  ara::log::LogInfo() << "ApogeeService: Running in event-driven mode.";
+
+  auto apogeeThread = std::jthread([this](const std::stop_token& token){
+    while (!token.stop_requested()) {
+      EvaluateApogee();
+      core::condition::wait_for(std::chrono::milliseconds(kAlgorithm_delay_ms), token);
+    }
+  });
+
+  while (!token.stop_requested()) {
+    this->service_ipc.newApogeeDetected.Update(is_apogee_detected);
+    this->service_udp.newApogeeDetected.Update(is_apogee_detected);
+    this->service_ipc.newMainParachuteDetected.Update(is_main_parachute_detected);
+    this->service_udp.newMainParachuteDetected.Update(is_main_parachute_detected);
+    core::condition::wait_for(std::chrono::milliseconds(1000), token);
+  }
+  this->service_ipc.StopOffer();
+  this->service_udp.StopOffer();
+  ara::log::LogInfo() << "ApogeeService: Stopped.";
+
+  return 0;
+}
+
+}  // namespace apps
+}  // namespace srp

apps/fc/apogee_service/apogee_app.hpp

@@ -0,0 +1,52 @@
+/**
+ * @file apogee_app.hpp
+ * @author Wiktor Laska
+ * @version 0.1
+ * @date 2026-04-25
+ * @copyright Copyright (c) 2026
+ */
+#ifndef APPS_FC_APOGEE_SERVICE_APOGEE_APP_HPP_
+#define APPS_FC_APOGEE_SERVICE_APOGEE_APP_HPP_
+
+#include <atomic>
+#include <chrono>  // NOLINT
+#include <memory>
+#include <map>
+
+#include "ara/exec/adaptive_application.h"
+#include "srp/apps/ApogeeDetectServiceSkeleton.h"
+#include "srp/env/EnvAppFc/EnvAppFcHandler.h"
+#include "core/apogee/ApogeeDetector.h"
+
+namespace srp {
+namespace apps {
+
+class ApogeeService : public ara::exec::AdaptiveApplication {
+ private:
+  env::EnvAppFcProxy env_service_proxy;
+  std::shared_ptr<env::EnvAppFcHandler> env_service_handler;
+  apps::ApogeeDetectServiceSkeleton service_ipc;
+  apps::ApogeeDetectServiceSkeleton service_udp;
+  RealTimeApogee apogee_detector_{15, -0.5, 0.0};
+  std::atomic<bool> is_apogee_detected;
+  std::atomic<bool> is_main_parachute_detected;
+  std::atomic<double> latest_height_;
+  std::atomic<double> latest_velocity_;
+  std::atomic<bool> first_imu_measurement_;
+
+  void SomeIpInit();
+  void EvaluateApogee();
+
+ protected:
+  int Run(const std::stop_token &token) override;
+  int Initialize(const std::map<ara::core::StringView, ara::core::StringView> parms) override;
+
+ public:
+  ApogeeService();
+  ~ApogeeService() = default;
+};
+
+}  // namespace apps
+}  // namespace srp
+
+#endif  // APPS_FC_APOGEE_SERVICE_APOGEE_APP_HPP_

apps/fc/apogee_service/main.cc

@@ -0,0 +1,16 @@
+/**
+ * @file main.cc
+ * @author Wiktor Laska   
+ * @brief 
+ * @version 0.1
+ * @date 2025-04-07
+ * 
+ * @copyright Copyright (c) 2026
+ * 
+ */
+#include "apps/fc/apogee_service/apogee_app.hpp"
+#include "ara/exec/adaptive_lifecycle.h"
+int main(int argc, char const *argv[]) {
+  return ara::exec::RunAdaptiveLifecycle<srp::apps::ApogeeService>(argc,
+                                                                      argv);
+}

apps/fc/logger_service/data_type.cpp

@@ -17,77 +17,77 @@ namespace srp {
 namespace logger {
 
 namespace {
-constexpr auto kCsvHeader =
-    "TIMESTAMP;BOARD_TEMP1;BOARD_TEMP2;BOARD_TEMP3;BME_TEMP;BME_HUMIDITY;"
-    "BME_ALTITUDE;CPU_USAGE;MEM_USAGE;DISK_UTILIZATION";
+  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";
 }
 
 std::string Data_t::get_header() {
   return kCsvHeader;
 }
 
 std::string Data_t::to_string(const std::string& timestamp) {
-  std::stringstream res;
-  res << std::fixed << std::setprecision(2);
-  res << timestamp << ";";
+  const auto t1 = board_temp1_.load(std::memory_order_relaxed);
+  const auto t2 = board_temp2_.load(std::memory_order_relaxed);
+  const auto t3 = board_temp3_.load(std::memory_order_relaxed);
 
-  std::shared_lock<std::shared_mutex> lock(mutex_);
-  res << board_temp1_ << ";";
-  res << board_temp2_ << ";";
-  res << board_temp3_ << ";";
-  res << bme_temp_ << ";";
-  res << bme_humidity_ << ";";
-  res << bme_altitude_ << ";";
-  res << cpu_usage_ << ";";
-  res << mem_usage_ << ";";
-  res << disk_utilization_;
-  return res.str();
-}
+  const auto bme_temp = bme_temp_.load(std::memory_order_relaxed);
+  const auto bme_hum  = bme_humidity_.load(std::memory_order_relaxed);
+  const auto bme_alt  = bme_altitude_.load(std::memory_order_relaxed);
 
-void Data_t::SetBoardTemp1(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  board_temp1_ = temp;
-}
+  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);
 
-void Data_t::SetBoardTemp2(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  board_temp2_ = temp;
+  std::stringstream res;
+  res << std::fixed << std::setprecision(2);
+  res << timestamp << kCsv_separator;
+  res << t1 << kCsv_separator;
+  res << t2 << kCsv_separator;
+  res << t3 << kCsv_separator;
+  res << bme_temp << kCsv_separator;
+  res << bme_hum << kCsv_separator;
+  res << bme_alt << kCsv_separator;
+  res << cpu << kCsv_separator;
+  res << mem << kCsv_separator;
+  res << disk << kCsv_separator;
+  res << apogee << kCsv_separator;
+  res << mainParachute << kCsv_separator;
+  return res.str();
 }
 
-void Data_t::SetBoardTemp3(const tempType& temp) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  board_temp3_ = temp;
+void Data_t::SetBoardTemp1(tempType temp) {
+  board_temp1_.store(temp, std::memory_order_relaxed);
 }
 
-void Data_t::SetBmeTemp(const bmeType& value) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  bme_temp_ = value;
+void Data_t::SetBoardTemp2(tempType temp) {
+  board_temp2_.store(temp, std::memory_order_relaxed);
 }
 
-void Data_t::SetBmeHumidity(const bmeType& value) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  bme_humidity_ = value;
+void Data_t::SetBoardTemp3(tempType temp) {
+  board_temp3_.store(temp, std::memory_order_relaxed);
 }
 
-void Data_t::SetBmeAltitude(const bmeType& value) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  bme_altitude_ = value;
+void Data_t::SetBmeData(const float temp, const float humidity, const float altitude) {
+  bme_temp_.store(temp, std::memory_order_relaxed);
+  bme_humidity_.store(humidity, std::memory_order_relaxed);
+  bme_altitude_.store(altitude, std::memory_order_relaxed);
 }
-
-void Data_t::SetCpuUsage(const systemStatType& value) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  cpu_usage_ = value;
+void Data_t::SetSystemUsage(const float cpu_usage, const float mem_usage, const float disk_usage) {
+  sys_cpu_usage_.store(cpu_usage, std::memory_order_relaxed);
+  sys_mem_usage_.store(mem_usage, std::memory_order_relaxed);
+  sys_disk_utilization_.store(disk_usage, std::memory_order_relaxed);
 }
 
-void Data_t::SetMemUsage(const systemStatType& value) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  mem_usage_ = value;
+void Data_t::SetApogeeDetected(const bool apogee_detected) {
+  apogee_detected_.store(apogee_detected, std::memory_order_relaxed);
 }
 
-void Data_t::SetDiskUtilization(const systemStatType& value) {
-  std::unique_lock<std::shared_mutex> lock(mutex_);
-  disk_utilization_ = value;
+void Data_t::SetMainParachuteDetected(const bool main_parachute_detected) {
+  main_parachute_detected_.store(main_parachute_detected, std::memory_order_relaxed);
 }
-
 }  // namespace logger
 }  // namespace srp