From 2664936292005f4f91782a7845c0a02def301689 Mon Sep 17 00:00:00 2001 From: Mateusz Krajewski Date: Tue, 15 Apr 2025 17:24:26 +0200 Subject: [PATCH] add main app skeleton --- apps/fc/main_service/BUILD | 25 ++++ apps/fc/main_service/main.cc | 18 +++ apps/fc/main_service/mainService.cpp | 64 +++++++++++ apps/fc/main_service/mainService.hpp | 52 +++++++++ apps/fc/main_service/rocketController.cpp | 120 ++++++++++++++++++++ apps/fc/main_service/rocketController.hpp | 40 +++++++ apps/fc/main_service/rocket_state.h | 61 ++++++++++ apps/fc/main_service/service.hpp | 76 +++++++++++++ deployment/apps/fc/main_app/BUILD | 34 ++++++ deployment/apps/fc/main_app/app_config.json | 58 ++++++++++ deployment/system_definition | 2 +- 11 files changed, 549 insertions(+), 1 deletion(-) create mode 100644 apps/fc/main_service/BUILD create mode 100644 apps/fc/main_service/main.cc create mode 100644 apps/fc/main_service/mainService.cpp create mode 100644 apps/fc/main_service/mainService.hpp create mode 100644 apps/fc/main_service/rocketController.cpp create mode 100644 apps/fc/main_service/rocketController.hpp create mode 100644 apps/fc/main_service/rocket_state.h create mode 100644 apps/fc/main_service/service.hpp create mode 100644 deployment/apps/fc/main_app/BUILD create mode 100644 deployment/apps/fc/main_app/app_config.json diff --git a/apps/fc/main_service/BUILD b/apps/fc/main_service/BUILD new file mode 100644 index 00000000..f143771a --- /dev/null +++ b/apps/fc/main_service/BUILD @@ -0,0 +1,25 @@ +cc_binary( + name = "MainService", + srcs = [ + "main.cc", + "mainService.cpp", + "mainService.hpp", + "service.hpp", + "rocket_state.h", + "rocketController.cpp", + "rocketController.hpp", + ], + visibility = [ + "//deployment/apps/fc/main_app:__subpackages__" + ], + deps = [ + "@srp_platform//ara/exec:adaptive_application_lib", + "@srp_platform//ara/diag:uds_lib", + "@srp_platform//ara/log", + "//core/common:condition_lib", + "//deployment/apps/fc/main_app:someip_lib", + "//deployment/apps/fc/main_app:ara", + "//core/common:common_types", + "//core/timestamp:timestamp_controller", + ], +) diff --git a/apps/fc/main_service/main.cc b/apps/fc/main_service/main.cc new file mode 100644 index 00000000..4cb25836 --- /dev/null +++ b/apps/fc/main_service/main.cc @@ -0,0 +1,18 @@ +/** + * @file main.cc + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2024-03-26 + * + * @copyright Copyright (c) 2024 + * + */ +#include "ara/exec/adaptive_lifecycle.h" +#include "apps/fc/main_service/mainService.hpp" + +int main(int argc, char const *argv[]) { + // setsid(); + return ara::exec::RunAdaptiveLifecycle(argc, + argv); +} diff --git a/apps/fc/main_service/mainService.cpp b/apps/fc/main_service/mainService.cpp new file mode 100644 index 00000000..7063333a --- /dev/null +++ b/apps/fc/main_service/mainService.cpp @@ -0,0 +1,64 @@ +/** + * @file mainService.cpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-14 + * + * @copyright Copyright (c) 2025 + * + */ +#include +#include +#include +#include + +#include "ara/exec/adaptive_application.h" +#include "apps/fc/main_service/service.hpp" +#include "apps/fc/main_service/mainService.hpp" +#include "core/common/condition.h" +#include "apps/fc/main_service/rocket_state.h" +namespace srp { +namespace service { +namespace { +constexpr auto kService_ipc_name = "srp/apps/MainService/MainService_ipc"; +constexpr auto kService_udp_name = "srp/apps/RecoveryService/MainService_udp"; +using RocketState_t = apps::RocketState_t; +const auto kMain_loop_delay_ms = 10; +const auto kSend_event_time = 1000; +} + +MainService::MainService() {} + +int MainService::Initialize(const std::map + parms) { + service_ipc = std::make_unique( + ara::core::InstanceSpecifier(kService_ipc_name)); + service_udp = std::make_unique( + ara::core::InstanceSpecifier(kService_udp_name)); + controller.Init(); + return 0; +} +int MainService::Run(const std::stop_token& token) { + service_ipc->StartOffer(); + service_udp->StartOffer(); + auto last_send = std::chrono::high_resolution_clock::now(); + auto state = apps::RocketState::GetInstance(); + while (!token.stop_requested()) { + controller.Loop(); + auto elapsed = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - last_send); + if (elapsed.count() > kSend_event_time) { + auto val = static_cast(state->GetState()); + this->service_ipc->CurrentModeStatusEvent.Update(val); + this->service_udp->CurrentModeStatusEvent.Update(val); + last_send = std::chrono::high_resolution_clock::now(); + } + core::condition::wait_for(std::chrono::milliseconds(kMain_loop_delay_ms), token); + } + service_ipc->StopOffer(); + service_udp->StopOffer(); + return 0; +} +} // namespace service +} // namespace srp diff --git a/apps/fc/main_service/mainService.hpp b/apps/fc/main_service/mainService.hpp new file mode 100644 index 00000000..d59fb752 --- /dev/null +++ b/apps/fc/main_service/mainService.hpp @@ -0,0 +1,52 @@ +/** + * @file mainService.hpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-14 + * + * @copyright Copyright (c) 2025 + * + */ +#ifndef APPS_FC_MAIN_SERVICE_MAINSERVICE_HPP_ +#define APPS_FC_MAIN_SERVICE_MAINSERVICE_HPP_ +#include +#include +#include +#include + +#include "ara/exec/adaptive_application.h" +#include "apps/fc/main_service/service.hpp" +#include "apps/fc/main_service/rocket_state.h" +#include "core/common/error_code.h" +#include "apps/fc/main_service/rocketController.hpp" +namespace srp { +namespace service { +class MainService final : public ara::exec::AdaptiveApplication { + private: + std::unique_ptr service_ipc; + std::unique_ptr service_udp; + apps::RocketController controller; + protected: + /** + * @brief This function is called to initialiaze the application + * + * @param parms map with parms + */ + int Initialize(const std::map + parms) override; + /** + * @brief This function is called to launch the application + * + * @param token stop token + */ + int Run(const std::stop_token& token) override; + + public: + ~MainService() = default; + MainService(); +}; + +} // namespace service +} // namespace srp +#endif // APPS_FC_MAIN_SERVICE_MAINSERVICE_HPP_ diff --git a/apps/fc/main_service/rocketController.cpp b/apps/fc/main_service/rocketController.cpp new file mode 100644 index 00000000..94fabe95 --- /dev/null +++ b/apps/fc/main_service/rocketController.cpp @@ -0,0 +1,120 @@ +/** + * @file rocketController.cpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-14 + * + * @copyright Copyright (c) 2025 + * + */ +#include "apps/fc/main_service/rocketController.hpp" +#include "core/common/condition.h" +namespace srp { +namespace apps { +namespace { + constexpr auto kCutdown_end_time_ms = 9500; + constexpr auto kLoss_conn_time_to_drop_fuel = 10000; + constexpr auto kPrimer_activation_delay_ms = 100; +} +RocketController::RocketController() { + rocket_state_ = apps::RocketState::GetInstance(); + last_state_ = rocket_state_->GetState(); + timestamp_.Init(); +} +void RocketController::Loop() { + auto now_state = rocket_state_->GetState(); + switch (now_state) { + case RocketState_t::INIT: + if (this->InitializeCompleted()) { + rocket_state_->SetState(RocketState_t::DISARM); + } + break; + case RocketState_t::DISARM: + break; + case RocketState_t::TANK: + if (last_state_ != RocketState_t::TANK) { + ActivateTankActuators(); + } + break; + case RocketState_t::ARM: + if (last_state_ != RocketState_t::ARM) { + ArmRocket(); + } + break; + case RocketState_t::CUTDOWN: { + static int start_timstamp; + if (last_state_ != RocketState_t::CUTDOWN) { + auto val_opt = timestamp_.GetNewTimeStamp(); + if (val_opt.has_value()) { + start_timstamp = val_opt.value(); + } + break; + } + auto now = timestamp_.GetNewTimeStamp(); + if (!now.has_value()) { + break; + } + if (timestamp_.GetDeltaTime(now.value(), start_timstamp) >= kCutdown_end_time_ms) { + this->rocket_state_->SetState(RocketState_t::CUTDOWN_END); + break; + } + break; + } + case RocketState_t::CUTDOWN_END: + this->CutdownEndSeq(); + this->rocket_state_->SetState(RocketState_t::FLIGHT); + break; + case RocketState_t::FLIGHT: + // TODO(simba) IDK thath we need something + break; + case RocketState_t::FALL: + if (last_state_ != RocketState_t::FALL) { + // TODO(simba) call Ref Parachute open + break; + } + + break; + case RocketState_t::LANDED: + if (last_state_ != RocketState_t::LANDED) { + // TODO(simba) call Main Parachute open + // TODO(simba) open all actuator and disable power on actuator + } + break; + case RocketState_t::ABORD: + // TODO(simba) open vent valve + // TODO(simba) disable all actuator power + break; + case RocketState_t::LOST_CONN: { + // NOW UNUSED + // if (last_state_ != RocketState_t::LOST_CONN) { + // LossConnSeq(); + // } + break; + } +} +last_state_ = now_state; +} + +core::ErrorCode RocketController::InitializeCompleted() { + return core::ErrorCode::kOk; +} +core::ErrorCode RocketController::ActivateTankActuators() { + return core::ErrorCode::kOk; +} +core::ErrorCode RocketController::ArmRocket() { + // enable power on all stages + return core::ErrorCode::kOk; +} +core::ErrorCode RocketController::CutdownEndSeq() { + // open main valve + std::this_thread::sleep_for(std::chrono::milliseconds(kPrimer_activation_delay_ms)); + // activate primer + return core::ErrorCode::kOk; +} +core::ErrorCode RocketController::LossConnSeq() { + return core::ErrorCode::kOk; +} + +} // namespace apps +} // namespace srp diff --git a/apps/fc/main_service/rocketController.hpp b/apps/fc/main_service/rocketController.hpp new file mode 100644 index 00000000..03632b72 --- /dev/null +++ b/apps/fc/main_service/rocketController.hpp @@ -0,0 +1,40 @@ +/** + * @file rocketController.hpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-14 + * + * @copyright Copyright (c) 2025 + * + */ +#ifndef APPS_FC_MAIN_SERVICE_ROCKETCONTROLLER_HPP_ +#define APPS_FC_MAIN_SERVICE_ROCKETCONTROLLER_HPP_ +#include +#include "apps/fc/main_service/rocket_state.h" +#include "core/common/error_code.h" +#include "core/timestamp/timestamp_driver.hpp" +namespace srp { +namespace apps { + +class RocketController { + private: + std::shared_ptr rocket_state_; + apps::RocketState_t last_state_; + srp::core::timestamp::TimestampController timestamp_; + core::ErrorCode InitializeCompleted(); + core::ErrorCode ActivateTankActuators(); + core::ErrorCode ArmRocket(); + core::ErrorCode CutdownEndSeq(); + core::ErrorCode LossConnSeq(); + + public: + void Loop(); + void Init() {} + RocketController(); +}; + +} // namespace apps +} // namespace srp + +#endif // APPS_FC_MAIN_SERVICE_ROCKETCONTROLLER_HPP_ diff --git a/apps/fc/main_service/rocket_state.h b/apps/fc/main_service/rocket_state.h new file mode 100644 index 00000000..15e25c89 --- /dev/null +++ b/apps/fc/main_service/rocket_state.h @@ -0,0 +1,61 @@ +/** + * @file rocket_state.h + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-14 + * + * @copyright Copyright (c) 2025 + * + */ +#ifndef APPS_FC_MAIN_SERVICE_ROCKET_STATE_H_ +#define APPS_FC_MAIN_SERVICE_ROCKET_STATE_H_ +#include // NOLINT +#include +#include +#include +namespace srp { +namespace apps { +class RocketState; +static std::shared_ptr rocket_state = nullptr; + +enum RocketState_t { + INIT = 0, + DISARM = 1, + TANK = 2, + ARM = 3, + CUTDOWN = 4, + CUTDOWN_END = 5, + FLIGHT = 6, + FALL = 7, + LANDED = 8, + ABORD = 50, + LOST_CONN = 52 +}; + +class RocketState { + private: + RocketState_t state_; + std::shared_mutex mtx_; + public: + static std::shared_ptr GetInstance() { + if (rocket_state == nullptr) { + rocket_state = std::make_shared(); + } + return rocket_state; + } + RocketState_t GetState() { + std::shared_lock lock(mtx_); + return this->state_; + } + void SetState(const RocketState_t state) { + std::unique_lock lock(mtx_); + this->state_ = state; + } +}; + + +} // namespace apps +} // namespace srp + +#endif // APPS_FC_MAIN_SERVICE_ROCKET_STATE_H_ diff --git a/apps/fc/main_service/service.hpp b/apps/fc/main_service/service.hpp new file mode 100644 index 00000000..674ed566 --- /dev/null +++ b/apps/fc/main_service/service.hpp @@ -0,0 +1,76 @@ +/** + * @file service.hpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-14 + * + * @copyright Copyright (c) 2025 + * + */ +#ifndef APPS_FC_MAIN_SERVICE_SERVICE_HPP_ +#define APPS_FC_MAIN_SERVICE_SERVICE_HPP_ +#include +#include +#include +#include "srp/apps/MainServiceSkeleton.h" +#include "apps/fc/main_service/rocket_state.h" +#include "ara/log/log.h" +namespace srp { +namespace apps { +static const std::unordered_map> allowed_transitions{ + {RocketState_t::INIT, {RocketState_t::DISARM}}, + {RocketState_t::DISARM, {RocketState_t::TANK, RocketState_t::ABORD}}, + {RocketState_t::TANK, {RocketState_t::ARM, RocketState_t::ABORD}}, + {RocketState_t::ARM, {RocketState_t::CUTDOWN, RocketState_t::ABORD}}, + {RocketState_t::CUTDOWN, {RocketState_t::CUTDOWN_END, RocketState_t::ABORD}}, + {RocketState_t::CUTDOWN_END, {RocketState_t::FLIGHT}}, + {RocketState_t::FLIGHT, {RocketState_t::FALL}}, + {RocketState_t::FALL, {RocketState_t::LANDED}}, + {RocketState_t::ABORD, {RocketState_t::DISARM}}, + {RocketState_t::LOST_CONN, {RocketState_t::DISARM}}, +}; +class MyMainServiceSkeleton: public MainServiceSkeleton { + private: + std::shared_ptr state; + inline void CaptureError(const RocketState_t req_state, const RocketState_t actual_state) const { + if (req_state == RocketState_t::CUTDOWN_END && actual_state == RocketState_t::CUTDOWN) { + ara::log::LogWarn() << + "CUTDOWN_END from CUTDOWN:This change needs to be called automatically by mainService, not from someip!"; + } + if (req_state == RocketState_t::DISARM && actual_state == RocketState_t::LOST_CONN) { + ara::log::LogWarn() << + "DISARM from LOST_CONN: This change needs to be called automatically by mainService, not from someip!"; + } + if (req_state == RocketState_t::DISARM && actual_state == RocketState_t::INIT) { + ara::log::LogWarn() << + "DISARM from INIT: This change needs to be called automatically by mainService, not from someip!"; + } + } + + public: + explicit MyMainServiceSkeleton(const ara::core::InstanceSpecifier& instance): + MainServiceSkeleton{instance} { + state = RocketState::GetInstance(); + } + virtual ~MyMainServiceSkeleton() = default; + + protected: + ara::core::Result setMode(const std::uint8_t& in_parm) override { + const auto req_state = static_cast(in_parm); + const auto actual_state = state->GetState(); + this->CaptureError(req_state, actual_state); + const auto it = allowed_transitions.find(actual_state); + if (it != allowed_transitions.end() && + std::find(it->second.begin(), it->second.end(), req_state) != it->second.end()) { + state->SetState(req_state); + CurrentModeStatusEvent.Update(static_cast(req_state)); + return true; + } + return false; + } +}; +} // namespace apps +} // namespace srp + +#endif // APPS_FC_MAIN_SERVICE_SERVICE_HPP_ diff --git a/deployment/apps/fc/main_app/BUILD b/deployment/apps/fc/main_app/BUILD new file mode 100644 index 00000000..e9e00247 --- /dev/null +++ b/deployment/apps/fc/main_app/BUILD @@ -0,0 +1,34 @@ +load("@srp_platform//tools/model_generator/ara:adaptive_application.bzl", "adaptive_application", "ara_runtime_lib", "ara_someip_lib") + + +filegroup( + name = "instance", + srcs = [ + "app_config.json", + "//deployment/system_definition/someip/fc/main_service:service_someipy", + ], + visibility = [ + "//apps/fc/main_service:__subpackages__", + "//deployment/apps/fc/main_service:__subpackages__", + ], +) + +ara_someip_lib( + name = "someip_lib", + model_src = ["//deployment/apps/fc/main_app:instance"], + visibility = ["//apps/fc/main_service:__subpackages__"], +) + +ara_runtime_lib( + name = "ara", + model_src = ["//deployment/apps/fc/main_app:instance"], + visibility = ["//apps/fc/main_service:__subpackages__"], +) + +adaptive_application( + name = "MainApp", + bin = "//apps/fc/main_service:MainService", + model_src = ["//deployment/apps/fc/main_app:instance"], + visibility = ["//deployment/cpu/fc:__subpackages__"], +) + diff --git a/deployment/apps/fc/main_app/app_config.json b/deployment/apps/fc/main_app/app_config.json new file mode 100644 index 00000000..1da70613 --- /dev/null +++ b/deployment/apps/fc/main_app/app_config.json @@ -0,0 +1,58 @@ +{ + "include": [ + "deployment/system_definition/someip/fc/main_service/service.json" + ], + "package": "srp.apps", + "adaptive_application": { + "MainApp": { + "app": { + "functional_groups":[ + "Running", + "SafetyMode" + ], + "parms": "", + "logger": { + "app_id": "MAIN", + "app_des": "glowna aplikacji rakiety", + "log_level": "kInfo", + "log_mode": "kRemote", + "ctx": [ + { + "ctx_id": "ara", + "log_level": "kInfo", + "ctx_des": "Default ctx for ara" + }, + { + "ctx_id": "acom", + "log_level": "kInfo", + "ctx_des": "Default ctx for ara::com" + }, + { + "ctx_id": "adiag", + "log_level": "kInfo", + "ctx_des": "Default ctx for ara::diag" + }, + { + "ctx_id": "exec", + "log_level": "kDebug", + "ctx_des": "Default ctx for ara::exec" + } + ] + } + }, + "provide": [ + { + "name": "MainService as MainService_ipc", + "on": "ipc", + "instance": 2 + }, + { + "name": "MainService as MainService_udp", + "on": "udp", + "port": "10002", + "instance": 1 + } + ] + } + } +} \ No newline at end of file diff --git a/deployment/system_definition b/deployment/system_definition index b1cfb173..58298afb 160000 --- a/deployment/system_definition +++ b/deployment/system_definition @@ -1 +1 @@ -Subproject commit b1cfb173709e9149c96a4e644564e9eed04e1ca2 +Subproject commit 58298afb324fbd5cdcc4814eec9278ec1980a0a9