diff --git a/apps/ec/radio_service/BUILD b/apps/ec/radio_service/BUILD new file mode 100644 index 00000000..86aac25f --- /dev/null +++ b/apps/ec/radio_service/BUILD @@ -0,0 +1,97 @@ +cc_binary( + name = "radio_service", + deps = [ + "@srp_platform//ara/exec:adaptive_application_lib", + ":telemetry_provider", + ":someip_controller", + "//deployment/apps/radio_app:ara", + ":radio_controller", + "//core/timestamp:timestamp_controller", + "@srp_platform//ara/log", + "//apps/fc/main_service:MainService", + "//core/onTimerCallback:on_timer_callback_lib", + ], + srcs = [ + "radio_app.cc", + "main.cc", + "gs_hb_guider.cpp", + "radio_app.h", + "gs_hb_guider.hpp", + ], + visibility = ["//visibility:public"], +) + +cc_library( + name = "someip_controller", + srcs = [ + "someip_controller.cpp", + ], + hdrs = [ + "someip_controller.hpp", + ], + visibility = ["//visibility:public"], + deps = [ + "@srp_platform//ara/log", + ":event_data", + "//deployment/apps/radio_app:someip_lib", + "//apps/fc/recovery_service:ParachuteController", + "//apps/ec/primer_service/controller:primer_controller", + ], +) + +cc_library( + name = "radio_controller", + srcs = [ + "radio_controller.cpp", + ], + hdrs = [ + "radio_controller.hpp", + ], + visibility = ["//visibility:public"], + deps = [ + "@srp_platform//ara/log", + ":event_data", + "@srp_mavlink//:simba_mavlink", + ":uart_driver", + "//core/common:wait_queue", + ], +) + +cc_library( + name = "telemetry_provider", + srcs = [ + "telemetry_provider.cpp", + ], + hdrs = [ + "telemetry_provider.hpp", + ], + visibility = ["//visibility:public"], + deps = [ + "@srp_mavlink//:simba_mavlink", + "@srp_platform//ara/log", + ":event_data", + "//core/timestamp:timestamp_controller", + "//apps/ec/primer_service/controller:primer_controller", + ], +) + +cc_library( + name = "event_data", + hdrs = ["event_data.h"], + srcs = ["event_data.cc"], + deps = [ + "@srp_mavlink//:simba_mavlink", + "//core/rocket_machine_state:rocket_state_base", + ], + visibility = ["//visibility:public"], +) + +cc_library( + name = "uart_driver", + hdrs = ["mavlink_uart_driver.hpp"], + srcs = ["mavlink_uart_driver.cpp"], + deps = [ + "//core/common:common_types", + ], + visibility = ["//visibility:public"], +) diff --git a/apps/ec/radio_service/event_data.cc b/apps/ec/radio_service/event_data.cc new file mode 100644 index 00000000..14b23dfa --- /dev/null +++ b/apps/ec/radio_service/event_data.cc @@ -0,0 +1,185 @@ +/** + * @file event_data.cc + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-02-28 + * + * @copyright Copyright (c) 2026 + * + */ +#include "apps/ec/radio_service/event_data.h" + +#include +#include + +namespace srp { +namespace apps { +namespace { + static std::shared_ptr event_data = nullptr; +} + +std::shared_ptr EventData::GetInstance() { + if (event_data == nullptr) { + event_data = std::make_shared(); + } + return event_data; +} + + core::rocketState::RocketState_t EventData::GetComputerState(const BoardType_e BoardType) { + switch (BoardType) { + case BoardType_e::EB: + return EBState_.get(); + case BoardType_e::MB: + return MBState_.get(); + default: + return core::rocketState::RocketState_t::INIT; + } + } + + void EventData::SetComputerState(const BoardType_e BoardType, const core::rocketState::RocketState_t state) { + switch (BoardType) { + case BoardType_e::EB: + EBState_.set(state); + break; + case BoardType_e::MB: + MBState_.set(state); + break; + default: + break; + } + } + + uint8_t EventData::GetPrimerStates() { + return primer_state_.get(); + } + void EventData::SetPrimerState(const uint8_t state) { + primer_state_.set(state); + } + + + uint8_t EventData::GetActuatorStates() { + return actuator_state_.get(); + } + void EventData::SetActuatorState(const uint8_t actuator_mask, const uint8_t state) { + actuator_state_.update([&](uint8_t& current_mask) { + if (state) { + current_mask |= actuator_mask; + } else { + current_mask &= static_cast(~actuator_mask); + } + }); + } + + computer_telemetry_t EventData::GetComputerTelemetry(const BoardType_e BoardType) { + if (BoardType == BoardType_e::EB) { + return eb_telemetry.get(); + } else { + return mb_telemetry.get(); + } + } + void EventData::SetComputerTemp(const BoardType_e BoardType, const uint8_t temp, const uint8_t index) { + if (index >= 3) { + return; + } + if (BoardType == BoardType_e::EB) { + eb_telemetry.update([&](auto& telemetry) { + telemetry.temps[index] = temp; + }); + } else if (BoardType == BoardType_e::MB) { + mb_telemetry.update([&](auto& telemetry) { + telemetry.temps[index] = temp; + }); + } + } + void EventData::SetComputerCpuUsage(const BoardType_e BoardType, const uint8_t cpu_usage) { + if (BoardType == BoardType_e::EB) { + eb_telemetry.update([&](auto& telemetry) { + telemetry.cpu_usage = cpu_usage; + }); + } else if (BoardType == BoardType_e::MB) { + mb_telemetry.update([&](auto& telemetry) { + telemetry.cpu_usage = cpu_usage; + }); + } + } + void EventData::SetComputerMemUsage(const BoardType_e BoardType, const uint8_t mem_usage) { + if (BoardType == BoardType_e::EB) { + eb_telemetry.update([&](auto& telemetry) { + telemetry.mem_usage = mem_usage; + }); + } else if (BoardType == BoardType_e::MB) { + mb_telemetry.update([&](auto& telemetry) { + telemetry.mem_usage = mem_usage; + }); + } + } + + uint16_t EventData::GetTemp(const uint8_t index) { + if (index >= 3) { + return 0; + } + switch (index) { + case 0: + return tank_.get().lower_tank; + case 1: + return tank_.get().middle_tank; + case 2: + return tank_.get().upper_tank; + default: + return 0; + } + } + void EventData::SetTemp(const uint8_t index, const int16_t res) { + if (index >= 3) { + return; + } + tank_.update([&](auto& temp) { + switch (index) { + case 0: + temp.lower_tank = res; + break; + case 1: + temp.middle_tank = res; + break; + case 2: + temp.upper_tank = res; + break; + default: + break; + } + }); + } + + uint16_t EventData::GetPress() { + return tank_.get().pressure; + } + void EventData::SetPress(uint16_t res) { + tank_.update([&](auto& tank) { + tank.pressure = res; + }); + } + + mavlink_simba_gps_t EventData::GetGPS() { + return gps_.get(); + } + + void EventData::SetGPS(int32_t lon, int32_t lat, int32_t alt) { + mavlink_simba_gps_t gps_data; + gps_data.altitude = alt; + gps_data.lat = lat; + gps_data.lon = lon; + gps_.set(gps_data); + } + + int32_t EventData::GetMaxAltitude() { + return max_altitude_.get().altitude; + } + void EventData::SetMaxAltitude(const int32_t alt) { + max_altitude_.update([&](auto& max_alt) { + max_alt.altitude = std::max(alt, max_alt.altitude); + }); + } + +} // namespace apps +} // namespace srp diff --git a/apps/ec/radio_service/event_data.h b/apps/ec/radio_service/event_data.h new file mode 100644 index 00000000..6e6d096d --- /dev/null +++ b/apps/ec/radio_service/event_data.h @@ -0,0 +1,103 @@ +/** + * @file event_data.h + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-05-07 + * + * @copyright Copyright (c) 2025 + * + */ +#ifndef APPS_EC_RADIO_SERVICE_EVENT_DATA_H_ +#define APPS_EC_RADIO_SERVICE_EVENT_DATA_H_ + +#include +#include // NOLINT +#include +#include +#include "simba/mavlink.h" +#include "core/rocket_machine_state/rocket_state.hpp" + +namespace srp { +namespace apps { + +template +class ThreadSafe { + public: + void set(const T& value) { + std::unique_lock lock(mtx_); + data_ = value; + } + + T get() const { + std::shared_lock lock(mtx_); + return data_; + } + + void update(std::function func) { + std::unique_lock lock(mtx_); + func(data_); + } + + private: + mutable std::shared_mutex mtx_; + T data_{}; +}; + +enum class BoardType_e {EB, MB}; + +struct computer_telemetry_t { + uint8_t temps[3]; + uint8_t cpu_usage; + uint8_t mem_usage; +}; + +class EventData { + private: + ThreadSafe tank_; + ThreadSafe gps_; + ThreadSafe imu_; + ThreadSafe max_altitude_; + ThreadSafe actuator_state_; + ThreadSafe primer_state_; + + ThreadSafe EBState_; + ThreadSafe MBState_; + + ThreadSafe eb_telemetry; + ThreadSafe mb_telemetry; + + public: + static std::shared_ptr GetInstance(); + + core::rocketState::RocketState_t GetComputerState(const BoardType_e BoardType); + void SetComputerState(const BoardType_e BoardType, const core::rocketState::RocketState_t state); + + uint8_t GetPrimerStates(); + void SetPrimerState(const uint8_t state); + + uint8_t GetActuatorStates(); + void SetActuatorState(const uint8_t actuator_mask, const uint8_t state); + + uint16_t GetTemp(const uint8_t index); + void SetTemp(const uint8_t index, const int16_t res); + + computer_telemetry_t GetComputerTelemetry(const BoardType_e BoardType); + void SetComputerTemp(const BoardType_e BoardType, const uint8_t temp, const uint8_t index); + void SetComputerCpuUsage(const BoardType_e BoardType, const uint8_t cpu_usage); + void SetComputerMemUsage(const BoardType_e BoardType, const uint8_t mem_usage); + + uint16_t GetPress(); + void SetPress(uint16_t res); + + mavlink_simba_gps_t GetGPS(); + void SetGPS(int32_t lon, int32_t lat, int32_t alt); + + int32_t GetMaxAltitude(); + void SetMaxAltitude(const int32_t alt); +}; + +} // namespace apps +} // namespace srp + +#endif // APPS_EC_RADIO_SERVICE_EVENT_DATA_H_ diff --git a/apps/ec/radio_service/gs_hb_guider.cpp b/apps/ec/radio_service/gs_hb_guider.cpp new file mode 100644 index 00000000..c12706f0 --- /dev/null +++ b/apps/ec/radio_service/gs_hb_guider.cpp @@ -0,0 +1,86 @@ +/** + * @file gs_hb_guider.cpp + * @author Mateusz Krajewski (Mateusz Krajewski) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#include "apps/ec/radio_service/gs_hb_guider.hpp" +#include // NOLINT +#include // NOLINT +#include +#include "ara/log/log.h" +#include "core/common/condition.h" +namespace srp { +namespace apps { +namespace radio { +namespace { + static constexpr auto kHbDelayWarning = 1.1; + static constexpr auto kDuration_from_last_hb_to_conn_lost_ms = 2 * 60 * 1000; + static constexpr auto kDuration_from_last_hb_to_abort_ms = 15 * 60 * 1000; +} + +void GSHeartbeatGuard::OnNewHbCallback() { + auto now = std::chrono::high_resolution_clock::now(); + { + std::lock_guard lock(hb_guard_mtx_); + if (!first_hb_received_) { + first_hb_received_ = true; + last_hb_time_ = now; + return; + } + last_hb_time_ = now; + } + auto duration = std::chrono::duration_cast(now - last_hb_time_).count(); + if (duration > (1000 * kHbDelayWarning)) { + ara::log::LogWarn() << "GS Heartbeat received with duration: " << duration; + } else { + ara::log::LogDebug() << "GS Heartbeat received with duration: " << duration; + } +} + +GSHeartbeatGuard::GSHeartbeatGuard(): first_hb_received_(false) { +} + +void GSHeartbeatGuard::Init(GsConLossCB cb) { + this->callback = cb; + guard_thread = std::jthread([this](const std::stop_token& t){ + GSHeartbeatGuardLoop(t); + }); +} + +void GSHeartbeatGuard::GSHeartbeatGuardLoop(const std::stop_token& t) { + auto broadcast_state = [this](RocketState_t state, const std::string& reason) { + ara::log::LogError() << "GS Connection Guard: " << reason << " -> Switching to " + << core::rocketState::to_string(state); + this->callback(state); + }; + bool first_hb_received; + timepoint last_hb_time; + while (!t.stop_requested()) { + core::condition::wait_for(std::chrono::milliseconds(1000), t); + { + std::lock_guard lock(hb_guard_mtx_); + first_hb_received = first_hb_received_; + last_hb_time = last_hb_time_; + } + if (!first_hb_received) { + continue; + } + auto now = std::chrono::high_resolution_clock::now(); + auto diff = std::chrono::duration_cast(now - last_hb_time).count(); + + if (diff >= kDuration_from_last_hb_to_abort_ms) { + broadcast_state(RocketState_t::ABORT, "Timeout ABORT"); + } else if (diff >= kDuration_from_last_hb_to_conn_lost_ms) { + broadcast_state(RocketState_t::CONNECTION_LOST, "Timeout CONN_LOST"); + } + } +} + +} // namespace radio +} // namespace apps +} // namespace srp diff --git a/apps/ec/radio_service/gs_hb_guider.hpp b/apps/ec/radio_service/gs_hb_guider.hpp new file mode 100644 index 00000000..b2a836af --- /dev/null +++ b/apps/ec/radio_service/gs_hb_guider.hpp @@ -0,0 +1,45 @@ +/** + * @file gs_hb_guider.hpp + * @author Mateusz Krajewski (Mateusz Krajewski) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#ifndef APPS_EC_RADIO_SERVICE_GS_HB_GUIDER_HPP_ +#define APPS_EC_RADIO_SERVICE_GS_HB_GUIDER_HPP_ +#include // NOLINT +#include +#include // NOLINT +#include // NOLINT +#include // NOLINT +#include "core/rocket_machine_state/rocket_state.hpp" +namespace srp { +namespace apps { +namespace radio { +using timepoint = std::chrono::_V2::high_resolution_clock::time_point; +using RocketState_t = core::rocketState::RocketState_t; +using GsConLossCB = std::function; +class GSHeartbeatGuard { + private: + std::mutex hb_guard_mtx_; + timepoint last_hb_time_; + bool first_hb_received_; + + GsConLossCB callback; + std::jthread guard_thread; + void GSHeartbeatGuardLoop(const std::stop_token& t); + + public: + GSHeartbeatGuard(); + void OnNewHbCallback(); + void Init(GsConLossCB cb); +}; + +} // namespace radio +} // namespace apps +} // namespace srp + +#endif // APPS_EC_RADIO_SERVICE_GS_HB_GUIDER_HPP_ diff --git a/apps/ec/radio_service/main.cc b/apps/ec/radio_service/main.cc new file mode 100644 index 00000000..e87071e3 --- /dev/null +++ b/apps/ec/radio_service/main.cc @@ -0,0 +1,17 @@ +/** + * @file main.cc + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-07 + * + * @copyright Copyright (c) 2025 + * + */ +#include "apps/ec/radio_service/radio_app.h" +#include "ara/exec/adaptive_lifecycle.h" +int main(int argc, char const *argv[]) { + // setsid(); + return ara::exec::RunAdaptiveLifecycle(argc, + argv); +} diff --git a/apps/ec/radio_service/mavlink_uart_driver.cpp b/apps/ec/radio_service/mavlink_uart_driver.cpp new file mode 100644 index 00000000..32f00e72 --- /dev/null +++ b/apps/ec/radio_service/mavlink_uart_driver.cpp @@ -0,0 +1,125 @@ +/** + * @file mavlink_uart_driver.cpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-03-14 + * + * @copyright Copyright (c) 2026 + * + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "apps/ec/radio_service/mavlink_uart_driver.hpp" +namespace srp { +namespace core { +namespace uart { + +bool MavUartDriver::WaitForData(int timeout_ms) { + struct pollfd fds[1]; + fds[0].fd = serial_port; + fds[0].events = POLLIN; + + int ret = poll(fds, 1, timeout_ms); + if (ret > 0) { + if (fds[0].revents & POLLIN) { + return true; + } + } + return false; +} + + +core::ErrorCode MavUartDriver::Write(const std::vector& data) { + auto n = write(serial_port, data.data(), data.size()); + if (n < 0) { + return core::ErrorCode::kConnectionError; + } + if (n != data.size()) { + return core::ErrorCode::kError; + } + return core::ErrorCode::kOk; +} + +/** + * @brief Open UART port + * + * @param portName + * @param baudrate + * @param timeout // value in deciseconds + * @return true + * @return false + */ +bool MavUartDriver::Open(const std::string& portName, const uint32_t& baudrate, const uint8_t timeout) { + serial_port = open(portName.c_str(), O_RDWR | O_NOCTTY); + if (serial_port == -1) { + return false; + } + struct termios tty; + if (tcgetattr(serial_port, &tty) != 0) { + return false; + } + + tty.c_cflag &= ~PARENB; // Brak parzystości + tty.c_cflag &= ~CSTOPB; // Jeden bit stopu + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; // 8 bitów danych + tty.c_cflag &= ~CRTSCTS; // Brak kontroli sprzętowej + tty.c_lflag &= ~ICANON; // Wyłączenie interpretacji nowych lini + + // // To Test 1 + tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP + // // To Test 2 + tty.c_lflag &= ~ECHO; // Disable echo + tty.c_lflag &= ~ECHOE; // Disable erasure + tty.c_lflag &= ~ECHONL; // Disable new-line echo + // + tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl + // + tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // Disable any special handling of received bytes + + tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars) + tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed + + tty.c_cflag |= CREAD | CLOCAL; + tty.c_cc[VTIME] = 10; // Wait for up to 1s (10 deciseconds), returning as soon as any data is received. + tty.c_cc[VMIN] = 0; + + cfsetospeed(&tty, baudrate); + cfsetispeed(&tty, baudrate); + + return tcsetattr(serial_port, TCSANOW, &tty) == 0; +} + + + +std::optional> MavUartDriver::Read(uint16_t size) { + const size_t bytes_to_read = (size == 0) ? 512 : size; + + std::vector res(bytes_to_read); + + ssize_t n = read(serial_port, res.data(), res.size()); + + if (n > 0) { + res.resize(n); + return res; + } + + return std::nullopt; +} +void MavUartDriver::Close() { + close(serial_port); +} + + +} // namespace uart +} // namespace core +} // namespace srp diff --git a/apps/ec/radio_service/mavlink_uart_driver.hpp b/apps/ec/radio_service/mavlink_uart_driver.hpp new file mode 100644 index 00000000..cf44639d --- /dev/null +++ b/apps/ec/radio_service/mavlink_uart_driver.hpp @@ -0,0 +1,63 @@ +/** + * @file mavlink_uart_driver.hpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-03-14 + * + * @copyright Copyright (c) 2026 + * + */ +#ifndef APPS_EC_RADIO_SERVICE_MAVLINK_UART_DRIVER_HPP_ +#define APPS_EC_RADIO_SERVICE_MAVLINK_UART_DRIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include "core/common/error_code.h" + +namespace srp { +namespace core { +namespace uart { +constexpr speed_t GetBaudrateConstant(uint32_t baudrate) { + switch (baudrate) { + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; + case 460800: return B460800; + case 500000: return B500000; + case 576000: return B576000; + case 921600: return B921600; + case 1000000: return B1000000; + case 1152000: return B1152000; + case 1500000: return B1500000; + case 2000000: return B2000000; + case 2500000: return B2500000; + case 3000000: return B3000000; + case 3500000: return B3500000; + case 4000000: return B4000000; + default: return 0; + } +} + +class MavUartDriver{ + private: + int serial_port; + + public: + bool WaitForData(int timeout_ms); + bool Open(const std::string& portName, const uint32_t& baudrate, const uint8_t timeout = 10); + std::optional> Read(const uint16_t size = 0); + core::ErrorCode Write(const std::vector& data); + void Close(); +}; + +} // namespace uart +} // namespace core +} // namespace srp + +#endif // APPS_EC_RADIO_SERVICE_MAVLINK_UART_DRIVER_HPP_ diff --git a/apps/ec/radio_service/radio_app.cc b/apps/ec/radio_service/radio_app.cc new file mode 100644 index 00000000..3a97637b --- /dev/null +++ b/apps/ec/radio_service/radio_app.cc @@ -0,0 +1,259 @@ +/** + * @file radio_app.cc + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-07 + * + * @copyright Copyright (c) 2025 + * + */ +#include "apps/ec/radio_service/radio_app.h" +#include +#include +#include +#include // NOLINT +#include "simba/mavlink.h" +#include "simba/simba.h" +#include "ara/log/log.h" +#include "core/common/condition.h" + +namespace srp { +namespace apps { +namespace { + static constexpr auto kHB_send_time_ms = 990; + static constexpr auto kMax_altitude_send_time_ms = 1000; + static constexpr auto kEngine_sensor_send_time_ms = 1000; + static constexpr auto kGps_send_time_ms = 1000; + static constexpr auto kComputer_telemetry_send_time_ms = 1000; + + static constexpr auto kRequired_mavlink_messages_to_change_state = 4; + + static constexpr auto kStatic_test_mode = false; + static constexpr auto kHeartBeatPinID = 2; + static constexpr auto kHeartbeat_time_ms = 500; + + static constexpr auto kService_ipc_instance = "srp/apps/RadioApp/RadioService_ipc"; + static constexpr auto kService_udp_instance = "srp/apps/RadioApp/RadioService_udp"; +} // namespace + + +void RadioApp::OnActuatorCMD(const mavlink_message_t& msg) { + // TODO(matikrajek42@gmail.com) IMplement this maybe +} + +void RadioApp::HBHangleActuators(const uint8_t values) { + std::shared_ptr servo_handler = someip_controller.GetServoServiceHandler(); + auto update_valve = [&](uint8_t gs_mask, uint8_t rocket_mask, const std::string& name, auto setter_func) { + uint8_t requested = ((values & gs_mask) != 0); + bool current = (event_data->GetActuatorStates() & rocket_mask) != 0; + + if (requested != current) { + if (!servo_handler) { + ara::log::LogWarn() << "Servo service handler not ready for " << name; + return; + } + ara::log::LogInfo() << "Changing " << name << " to " << (requested ? "ON" : "OFF"); + event_data->SetActuatorState(static_cast(rocket_mask), requested); + setter_func(requested); + } + }; + + auto eb_state = event_data->GetComputerState(BoardType_e::EB); + + if (eb_state == RocketState_t::ARM || (kStatic_test_mode && eb_state == RocketState_t::DISARM)) { + // Dump Valve + update_valve(SIMBA_GS_FLAGS_DUMP_VALVE, SIMBA_ACTUATOR_FLAGS_DUMP_VALVE, "DUMP_VALVE", + [&](uint8_t val) { servo_handler->SetDumpValue(val); }); + + // Vent Valve + uint8_t requested = ((values & SIMBA_GS_FLAGS_VENT_VALVE) != 0); + if (servo_handler) { + ara::log::LogInfo() << "Changing VENT_VALVE to " << (requested ? "ON" : "OFF"); + event_data->SetActuatorState(static_cast(SIMBA_ACTUATOR_FLAGS_VENT_VALVE), requested); + servo_handler->SetVentServoValue(requested); + } + } + + // TODO(matikrajek42@gmail.com) Add missing Cameras handler +} + +/** + * @brief Spełnienie wymagania konkursu FAROUT -> pojedyńcza wiadomość mavlink nie może wywołać zmiany stanu + * + * @param req_state + * @return true + * @return false + */ +bool RadioApp::IsStateChangeApproved(const RocketState_t req_state) { + if (req_state != req_rocket_state.first) { + req_rocket_state.first = req_state; + req_rocket_state.second = 1; + return false; + } + req_rocket_state.second += 1; + if (req_rocket_state.second >= kRequired_mavlink_messages_to_change_state) { + return true; + } + return false; +} + +void RadioApp::HBHangleState(const uint8_t values) { + auto req_state = telemetry_provider.GetReqRocketStateFromGSFlags(values); + if (!req_state.has_value()) { + return; + } + auto current_mb = event_data->GetComputerState(BoardType_e::MB); + auto current_eb = event_data->GetComputerState(BoardType_e::EB); + + auto engine_handler = someip_controller.GetEngineServiceHandler(); + auto main_handler = someip_controller.GetMainServiceHandler(); + if (!((engine_handler && (req_state.value() != current_eb)) || + ((main_handler && (req_state.value() != current_mb))))) { + return; + } + + if (!IsStateChangeApproved(req_state.value())) { + return; + } + + if (engine_handler) { + engine_handler->SetMode(static_cast(req_state.value())); + } else { + ara::log::LogWarn() << "Service handlers not ready in GS_HEARTBEAT (engine)."; + } + if (main_handler) { + main_handler->setMode(static_cast(req_state.value())); + } else { + ara::log::LogWarn() << "Service handlers not ready in GS_HEARTBEAT (main)."; + } + ara::log::LogInfo() << "Changing rocket state to " + + core::rocketState::to_string(req_state.value()); +} + +void RadioApp::OnGSHEARTBEAT(const mavlink_message_t& msg) { + heartbeat_controller.OnNewHbCallback(); + + auto timestamp = mavlink_msg_simba_gs_heartbeat_get_timestamp(&msg); + auto values = mavlink_msg_simba_gs_heartbeat_get_values(&msg); + ara::log::LogInfo() << "GS Heartbeat: ts=" + << timestamp << " flags=" << values; + + HBHangleState(values); + + HBHangleActuators(values); +} + +void RadioApp::OnRadioStatusMsg(const mavlink_message_t& msg) { + mavlink_radio_status_t radio_status; + mavlink_msg_radio_status_decode(&msg, &radio_status); + ara::log::LogWarn() + << "rxErrors: " << radio_status.rxerrors + << " TxFreeBuf: " << radio_status.txbuf + << " Fixed: " << radio_status.fixed + << " rssi: " << radio_status.rssi + << " remote rssi: " << radio_status.remrssi + << " noise: " << radio_status.noise + << " remote noise: " << radio_status.remnoise; + + RadioDataType someip_radio_data { + .rxerrors = radio_status.rxerrors, + .fixed = radio_status.fixed, + .rssi = radio_status.rssi, + .remrssi = radio_status.remrssi, + .txbuf = radio_status.txbuf, + .noise = radio_status.noise, + .remnoise = radio_status.remnoise + }; + + service_ipc.RadioStatusEvent.Update(someip_radio_data); + service_udp.RadioStatusEvent.Update(someip_radio_data); +} + +void RadioApp::RxMsgCallback(const mavlink_message_t& msg) { + switch (msg.msgid) { + case MAVLINK_MSG_ID_SIMBA_ACTUATOR_CMD: + OnActuatorCMD(msg); + break; + case MAVLINK_MSG_ID_SIMBA_GS_HEARTBEAT: + OnGSHEARTBEAT(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: + OnRadioStatusMsg(msg); + break; + default: + ara::log::LogInfo() << "Received unknown msg with ID: " << + msg.msgid << ", with size: " << msg.len; + break; + } +} + +int RadioApp::Run(const std::stop_token& token) { + radio_controller.Init([this](const mavlink_message_t& msg) { RxMsgCallback(msg); }, token); + heartbeat_controller.Init([this](const RocketState_t state) { + auto engine_handler = someip_controller.GetEngineServiceHandler(); + auto main_handler = someip_controller.GetMainServiceHandler(); + if (engine_handler) { + engine_handler->SetMode(static_cast(state)); + } + if (main_handler) { + main_handler->setMode(static_cast(state)); + } + }); + ara::log::LogDebug() << "RadioApp Run starting threads"; + timer_ctr_.Start(); + service_ipc.StartOffer(); + service_udp.StartOffer(); + + while (!token.stop_requested()) { + if (gpio_.SetPinValue(kHeartBeatPinID, 1, kHeartbeat_time_ms) != core::ErrorCode::kOk) { + ara::log::LogWarn() << "EngineApp::Run: Failed to toggle heartbeat pin"; + } + core::condition::wait_for(std::chrono::milliseconds(kHeartbeat_time_ms * 2), token); + } + timer_ctr_.Stop(); + service_ipc.StopOffer(); + service_udp.StopOffer(); + return core::ErrorCode::kOk; +} + +int RadioApp::Initialize(const std::map parms) { + event_data = EventData::GetInstance(); + // timer_ctr_.AddOnTimerCallback([this](){ + // auto hb = telemetry_provider.GetHeartbeatMsg(); + // if (hb.has_value()) { + // radio_controller.Push(hb.value()); + // } + // }, kHB_send_time_ms); + + timer_ctr_.AddOnTimerCallback([this](){ + // Max Altitude + radio_controller.Push(telemetry_provider.GetMaxAltitudeMsg()); + }, kMax_altitude_send_time_ms); + + // timer_ctr_.AddOnTimerCallback([this](){ + // // Tank Sensors + // radio_controller.Push(telemetry_provider.GetTankSensorsMsg()); + // }, kEngine_sensor_send_time_ms); + + timer_ctr_.AddOnTimerCallback([this](){ + // GPS + radio_controller.Push(telemetry_provider.GetGpsMsg()); + }, kGps_send_time_ms); + + // timer_ctr_.AddOnTimerCallback([this](){ + // // Computers telemetry + // radio_controller.Push(telemetry_provider.GetComputersTelemetryMsg()); + // }, kComputer_telemetry_send_time_ms); + + return core::ErrorCode::kOk; +} + +RadioApp::RadioApp(): + service_ipc(ara::core::InstanceSpecifier{kService_ipc_instance}), + service_udp(ara::core::InstanceSpecifier{kService_udp_instance}) { +} + +} // namespace apps +} // namespace srp diff --git a/apps/ec/radio_service/radio_app.h b/apps/ec/radio_service/radio_app.h new file mode 100644 index 00000000..c44b7cfa --- /dev/null +++ b/apps/ec/radio_service/radio_app.h @@ -0,0 +1,74 @@ +/** + * @file radio_app.h + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-07 + * + * @copyright Copyright (c) 2025 + * + */ + +#ifndef APPS_EC_RADIO_SERVICE_RADIO_APP_H_ +#define APPS_EC_RADIO_SERVICE_RADIO_APP_H_ + +#include +#include // NOLINT +#include +#include +#include +#include + +#include "ara/exec/adaptive_application.h" +#include "apps/ec/radio_service/event_data.h" +#include "srp/apps/FcRadioServiceSkeleton.h" +#include "mw/gpio_server/controller/gpio_controller.hpp" +#include "apps/ec/radio_service/telemetry_provider.hpp" +#include "apps/ec/radio_service/someip_controller.hpp" +#include "apps/ec/radio_service/radio_controller.hpp" +#include "apps/ec/radio_service/gs_hb_guider.hpp" +#include "core/onTimerCallback/onTimer.hpp" + + +namespace srp { +namespace apps { +using RocketState_t = core::rocketState::RocketState_t; +using timepoint = std::chrono::_V2::system_clock::time_point; + +class RadioApp : public ara::exec::AdaptiveApplication { + private: + radio::TelemetryProvider telemetry_provider; + radio::SomeIPController someip_controller; + radio::MavRadioController radio_controller; + gpio::GPIOController gpio_; + radio::GSHeartbeatGuard heartbeat_controller; + core::TimerController timer_ctr_; + + timepoint last_received_hb; + + bool IsStateChangeApproved(const RocketState_t req_state); + + std::pair req_rocket_state; + std::shared_ptr event_data; + + apps::FcRadioServiceSkeleton service_ipc; + apps::FcRadioServiceSkeleton service_udp; + + + void OnActuatorCMD(const mavlink_message_t& msg); + void HBHangleActuators(const uint8_t values); + void HBHangleState(const uint8_t values); + void OnGSHEARTBEAT(const mavlink_message_t& msg); + void RxMsgCallback(const mavlink_message_t& msg); + void OnRadioStatusMsg(const mavlink_message_t& msg); + + public: + int Run(const std::stop_token& token) override; + int Initialize(const std::map + parms) override; + RadioApp(); +}; +} // namespace apps +} // namespace srp + +#endif // APPS_EC_RADIO_SERVICE_RADIO_APP_H_ diff --git a/apps/ec/radio_service/radio_controller.cpp b/apps/ec/radio_service/radio_controller.cpp new file mode 100644 index 00000000..343ba28e --- /dev/null +++ b/apps/ec/radio_service/radio_controller.cpp @@ -0,0 +1,72 @@ +/** + * @file radio_controller.cpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#include "apps/ec/radio_service/radio_controller.hpp" +#include "ara/log/log.h" +namespace srp { +namespace apps { +namespace radio { +namespace { + static constexpr auto KRadio_UART_path = "/dev/ttyS1"; + static constexpr auto KRadio_UART_baudrate = core::uart::GetBaudrateConstant(57600); +} + +void MavRadioController::Push(const std::vector& data) { + UartTxQueue.Push(data); +} + +void MavRadioController::RxThreadLoop(const std::stop_token& t) { + mavlink_message_t msg; + mavlink_status_t status; + while (!t.stop_requested()) { + if (!uart_.WaitForData(250)) { + continue; + } + auto bytes_opt = uart_.Read(0); + if (!bytes_opt.has_value()) { + continue; + } + for (uint8_t i = 0; i < bytes_opt.value().size(); i++) { + if (!mavlink_parse_char(MAVLINK_COMM_0, bytes_opt.value()[i], &msg, &status)) { + continue; + } + mavl_logger.LogDebug() << "Parsed MAVLink msg id=" + << msg.msgid << " len=" << msg.len; + callback(msg); + } + } +} +void MavRadioController::TxThreadLoop(const std::stop_token& t) { + while (!t.stop_requested()) { + auto package_opt = UartTxQueue.Get(t); + if (!package_opt.has_value()) { + continue; + } + ara::log::LogWarn() << "Send: " << package_opt.value(); + uart_.Write(package_opt.value()); + } +} + +void MavRadioController::Init(RadioMsgCallback callback, const std::stop_token& t) { + if (!this->uart_.Open(KRadio_UART_path, KRadio_UART_baudrate, 10)) { + ara::log::LogError() << "Failed to open UART: " << KRadio_UART_path; + exit(1); + } + this->callback = callback; + uart_transmit_thread = std::jthread([this](const std::stop_token& t) { TxThreadLoop(t); }); + uart_receive_thread = std::jthread([this](const std::stop_token& t) { RxThreadLoop(t); }); +} + +MavRadioController::MavRadioController(): mavl_logger{ara::log::LoggingMenager::GetInstance()-> + CreateLogger("MAVL", "", ara::log::LogLevel::kDebug)} {} + +} // namespace radio +} // namespace apps +} // namespace srp diff --git a/apps/ec/radio_service/radio_controller.hpp b/apps/ec/radio_service/radio_controller.hpp new file mode 100644 index 00000000..b01b6316 --- /dev/null +++ b/apps/ec/radio_service/radio_controller.hpp @@ -0,0 +1,51 @@ +/** + * @file radio_controller.hpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#ifndef APPS_EC_RADIO_SERVICE_RADIO_CONTROLLER_HPP_ +#define APPS_EC_RADIO_SERVICE_RADIO_CONTROLLER_HPP_ +#include +#include +#include +#include +#include // NOLINT +#include "core/common/error_code.h" +#include "core/common/wait_queue.h" +#include "ara/log/logging_menager.h" +#include "apps/ec/radio_service/mavlink_uart_driver.hpp" +#include "simba/mavlink.h" + +namespace srp { +namespace apps { +namespace radio { + +using RadioMsgCallback = std::function; + +class MavRadioController { + private: + const ara::log::Logger& mavl_logger; + core::uart::MavUartDriver uart_; + core::WaitQueue, 20> UartTxQueue; + RadioMsgCallback callback; + std::jthread uart_transmit_thread; + std::jthread uart_receive_thread; + void RxThreadLoop(const std::stop_token& t); + void TxThreadLoop(const std::stop_token& t); + public: + void Push(const std::vector& data); + void TxLoop(); + void Init(RadioMsgCallback callback, const std::stop_token& t); + MavRadioController(); +}; + +} // namespace radio +} // namespace apps +} // namespace srp + +#endif // APPS_EC_RADIO_SERVICE_RADIO_CONTROLLER_HPP_ diff --git a/apps/ec/radio_service/someip_controller.cpp b/apps/ec/radio_service/someip_controller.cpp new file mode 100644 index 00000000..98613d28 --- /dev/null +++ b/apps/ec/radio_service/someip_controller.cpp @@ -0,0 +1,370 @@ +/** + * @file someip_controller.cpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#include "apps/ec/radio_service/someip_controller.hpp" +#include +#include +#include // NOLINT +#include "ara/log/logging_menager.h" + +namespace srp { +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"; +} // namespace + +SomeIPController::SomeIPController(): + someip_logger{ara::log::LoggingMenager::GetInstance()->CreateLogger("SOME", "", ara::log::LogLevel::kWarn)}, + event_data(EventData::GetInstance()), + primer_service_proxy{ara::core::InstanceSpecifier{kPrimer_service_path_name}}, + primer_service_handler{nullptr}, + servo_service_proxy{ara::core::InstanceSpecifier{kServo_service_path_name}}, + servo_service_handler{nullptr}, + env_service_proxy{ara::core::InstanceSpecifier{kEnv_service_path_name}}, + env_service_handler{nullptr}, + env_fc_service_proxy{ara::core::InstanceSpecifier{kEnv_fc_service_path_name}}, + env_fc_service_handler{nullptr}, + gps_service_proxy{ara::core::InstanceSpecifier{kGPS_service_path_name}}, + gps_service_handler{nullptr}, + main_service_handler{nullptr}, + main_service_proxy{ara::core::InstanceSpecifier{kMain_service_path_name}}, + engine_service_handler{nullptr}, + engine_service_proxy{ara::core::InstanceSpecifier{kEngine_service_path_name}}, + recovery_service_handler{nullptr}, + recovery_service_proxy{ara::core::InstanceSpecifier{kRecovery_service_path_name}} { + SomeIpInit(); +} +std::shared_ptr SomeIPController::GetMainServiceHandler() { + return main_service_handler; +} +std::shared_ptr SomeIPController::GetEngineServiceHandler() { + return engine_service_handler; +} + +std::shared_ptr SomeIPController::GetServoServiceHandler() { + return servo_service_handler; +} + +void SomeIPController::SomeIpInit() { + someip_logger.LogDebug() << "SomeIpInit started"; + this->env_service_proxy.StartFindService([this](auto handler) { + someip_logger.LogDebug() << "Env service handler discovered"; + this->env_service_handler = handler; + env_service_handler->newTempEvent_1.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Env newTempEvent_1, status=" + << status; + env_service_handler->newTempEvent_1.SetReceiveHandler([this] () { + auto res = env_service_handler->newTempEvent_1.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Env newTempEvent_1 sample: " << res.Value(); + event_data->SetTemp(0, res.Value()); + }); + }); + env_service_handler->newTempEvent_2.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Env newTempEvent_2, status=" + << status; + env_service_handler->newTempEvent_2.SetReceiveHandler([this] () { + auto res = env_service_handler->newTempEvent_2.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Env newTempEvent_2 sample: " << res.Value(); + event_data->SetTemp(1, res.Value()); + }); + }); + env_service_handler->newTempEvent_3.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Env newTempEvent_3, status=" + << status; + env_service_handler->newTempEvent_3.SetReceiveHandler([this] () { + auto res = env_service_handler->newTempEvent_3.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Env newTempEvent_3 sample: " << res.Value(); + event_data->SetTemp(2, res.Value()); + }); + }); + env_service_handler->newPressEvent.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Env newPressEvent, status=" + << status; + env_service_handler->newPressEvent.SetReceiveHandler([this] () { + auto res = env_service_handler->newPressEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Env newPressEvent sample: " << res.Value(); + event_data->SetPress(res.Value()); + }); + }); + env_service_handler->newBoardTempEvent1.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Env newBoardTempEvent1, status=" + << status; + env_service_handler->newBoardTempEvent1.SetReceiveHandler([this] () { + auto res = env_service_handler->newBoardTempEvent1.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Env newBoardTempEvent1 sample: " << res.Value(); + event_data->SetComputerTemp(BoardType_e::EB, 0, res.Value()); + }); + }); + env_service_handler->newBoardTempEvent2.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Env newBoardTempEvent2, status=" + << status; + env_service_handler->newBoardTempEvent2.SetReceiveHandler([this] () { + auto res = env_service_handler->newBoardTempEvent2.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Env newBoardTempEvent2 sample: " << res.Value(); + event_data->SetComputerTemp(BoardType_e::EB, 1, res.Value()); + }); + }); + env_service_handler->newBoardTempEvent3.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Env newBoardTempEvent3, status=" + << status; + env_service_handler->newBoardTempEvent3.SetReceiveHandler([this] () { + auto res = env_service_handler->newBoardTempEvent3.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Env newBoardTempEvent3 sample: " << res.Value(); + event_data->SetComputerTemp(BoardType_e::EB, 2, res.Value()); + }); + }); + }); + this->gps_service_proxy.StartFindService([this](auto handler) { + someip_logger.LogDebug() << "GPS service handler discovered"; + this->gps_service_handler = handler; + gps_service_handler->GPSStatusEvent.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to GPSStatusEvent, status=" + << status; + gps_service_handler->GPSStatusEvent.SetReceiveHandler([this] () { + auto res_opt = gps_service_handler->GPSStatusEvent.GetNewSamples(); + if (!res_opt.HasValue()) { + return; + } + auto res = res_opt.Value(); + someip_logger.LogDebug() << "GPSStatusEvent sample: lon=" + << res.longitude + << ", lat=" << res.latitude + << ", alt=" << res.altitude; + event_data->SetGPS(res.longitude, res.latitude, res.altitude); + }); + }); + }); + this->servo_service_proxy.StartFindService([this](auto handler) { + someip_logger.LogDebug() << "Servo service handler discovered"; + this->servo_service_handler = handler; + servo_service_handler->ServoStatusEvent.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to ServoStatusEvent, status=" + << status; + servo_service_handler->ServoStatusEvent.SetReceiveHandler([this] () { + auto res = servo_service_handler->ServoStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "ServoStatusEvent sample: " << res.Value(); + event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_MAIN_VALVE, res.Value()); + }); + }); + servo_service_handler->ServoVentStatusEvent.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to ServoVentStatusEvent, status=" + << status; + servo_service_handler->ServoVentStatusEvent.SetReceiveHandler([this] () { + auto res = servo_service_handler->ServoVentStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "ServoVentStatusEvent sample: " << res.Value(); + event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_VENT_VALVE, res.Value()); + }); + }); + servo_service_handler->ServoDumpStatusEvent.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to ServoDumpStatusEvent, status=" + << status; + servo_service_handler->ServoDumpStatusEvent.SetReceiveHandler([this] () { + auto res = servo_service_handler->ServoDumpStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "ServoDumpStatusEvent sample: " << res.Value(); + event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_DUMP_VALVE, res.Value()); + }); + }); + }); + this->recovery_service_proxy.StartFindService([this] (auto handler){ + someip_logger.LogDebug() << "Recovery service handler discovered"; + this->recovery_service_handler = handler; + recovery_service_handler->NewParachuteStatusEvent.Subscribe(1, [this](const uint8_t status){ + someip_logger.LogDebug() << "Subscribed to ParachuteStatusEvent, status=" + << status; + recovery_service_handler->NewParachuteStatusEvent.SetReceiveHandler([this] (){ + auto res = recovery_service_handler->NewParachuteStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "NewParachuteStatusEvent sample: " + << res.Value(); + switch (static_cast (res.Value())) { + case recovery::ParachuteState_t::CLOSED: + this->event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_RECOVERY_SERVO, 0); + this->event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_RECOVERY_LINECUTTER, 0); + break; + case recovery::ParachuteState_t::OPEN_REEFED: + case recovery::ParachuteState_t::OPENING_REEFED: + this->event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_RECOVERY_SERVO, 1); + this->event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_RECOVERY_LINECUTTER, 0); + break; + case recovery::ParachuteState_t::OPEN_UNREEFED: + case recovery::ParachuteState_t::OPENING_UNREEFED: + this->event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_RECOVERY_SERVO, 1); + this->event_data->SetActuatorState(SIMBA_ACTUATOR_FLAGS_RECOVERY_LINECUTTER, 1); + break; + default: + break; + } + }); + }); + }); + this->env_fc_service_proxy.StartFindService([this](auto handler) { + someip_logger.LogDebug() << "Env Flight Computer service handler discovered"; + this->env_fc_service_handler = handler; + env_fc_service_handler->newBME280Event.Subscribe(1, [this](const uint8_t status){ + someip_logger.LogDebug() << "Subscribed to BME280 event, status=" + << status; + env_fc_service_handler->newBME280Event.SetReceiveHandler([this] (){ + auto res = env_fc_service_handler->newBME280Event.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Altitude sample: " + << res.Value().altitude; + this->event_data->SetMaxAltitude(static_cast(res.Value().altitude)); + }); + }); + env_fc_service_handler->newBoardTempEvent_1.Subscribe(1, [this](const uint8_t status){ + someip_logger.LogDebug() << "Subscribed to newBoardTempEvent_1 event, status=" + << status; + env_fc_service_handler->newBoardTempEvent_1.SetReceiveHandler([this] (){ + auto res = env_fc_service_handler->newBoardTempEvent_1.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "newBoardTempEvent_1 sample: " + << res.Value(); + this->event_data->SetComputerTemp(BoardType_e::MB, 0, res.Value()); + }); + }); + env_fc_service_handler->newBoardTempEvent_2.Subscribe(1, [this](const uint8_t status){ + someip_logger.LogDebug() << "Subscribed to newBoardTempEvent_2 event, status=" + << status; + env_fc_service_handler->newBoardTempEvent_2.SetReceiveHandler([this] (){ + auto res = env_fc_service_handler->newBoardTempEvent_2.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "newBoardTempEvent_2 sample: " + << res.Value(); + this->event_data->SetComputerTemp(BoardType_e::MB, 1, res.Value()); + }); + }); + env_fc_service_handler->newBoardTempEvent_3.Subscribe(1, [this](const uint8_t status){ + someip_logger.LogDebug() << "Subscribed to newBoardTempEvent_3 event, status=" + << status; + env_fc_service_handler->newBoardTempEvent_3.SetReceiveHandler([this] (){ + auto res = env_fc_service_handler->newBoardTempEvent_3.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "newBoardTempEvent_3 sample: " + << res.Value(); + this->event_data->SetComputerTemp(BoardType_e::MB, 2, res.Value()); + }); + }); + }); + + // TODO(matikrajek42@gmail.com) ROCKET_CAMERAS + this->main_service_proxy.StartFindService([this](auto handler) { + someip_logger.LogDebug() << "Main service handler discovered"; + this->main_service_handler = handler; + main_service_handler->CurrentModeStatusEvent.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Main CurrentModeStatusEvent, status=" + << status; + main_service_handler->CurrentModeStatusEvent.SetReceiveHandler([this] () { + auto res = main_service_handler->CurrentModeStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Main CurrentModeStatusEvent sample: " + << core::rocketState::to_string(static_cast(res.Value())); + this->event_data->SetComputerState(BoardType_e::MB, + static_cast(res.Value())); + }); + }); + }); + this->engine_service_proxy.StartFindService(([this](auto handler) { + someip_logger.LogDebug() << "Engine service handler discovered"; + this->engine_service_handler = handler; + engine_service_handler->CurrentMode.Subscribe(1, [this](const uint8_t status) { + someip_logger.LogDebug() << "Subscribed to Engine CurrentMode, status=" << status; + engine_service_handler->CurrentMode.SetReceiveHandler([this] () { + auto res = engine_service_handler->CurrentMode.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Engine CurrentMode sample: " + << core::rocketState::to_string(static_cast(res.Value())); + this->event_data->SetComputerState(BoardType_e::EB, + static_cast(res.Value())); + }); + }); + })); + this->primer_service_proxy.StartFindService([this](auto handler) { + someip_logger.LogDebug() << "Primer service handler discovered"; + this->primer_service_handler = handler; + primer_service_handler->primeStatusEvent.SetReceiveHandler([this] () { + const auto res = primer_service_handler->primeStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + someip_logger.LogDebug() << "Primer state sample: " + << res.Value(); + this->event_data->SetPrimerState(res.Value()); + }); + }); + // TODO(matikrajek42@gmail.com) Write MB Temp After GrKo write Env App for FC +} + +SomeIPController::~SomeIPController() { + env_service_proxy.StopFindService(); + gps_service_proxy.StopFindService(); + primer_service_proxy.StopFindService(); + servo_service_proxy.StopFindService(); + recovery_service_proxy.StopFindService(); + main_service_proxy.StopFindService(); + engine_service_proxy.StopFindService(); + env_fc_service_proxy.StopFindService(); +} + +} // namespace radio +} // namespace apps +} // namespace srp diff --git a/apps/ec/radio_service/someip_controller.hpp b/apps/ec/radio_service/someip_controller.hpp new file mode 100644 index 00000000..0a10dcb1 --- /dev/null +++ b/apps/ec/radio_service/someip_controller.hpp @@ -0,0 +1,73 @@ +/** + * @file someip_controller.hpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#ifndef APPS_EC_RADIO_SERVICE_SOMEIP_CONTROLLER_HPP_ +#define APPS_EC_RADIO_SERVICE_SOMEIP_CONTROLLER_HPP_ +#include +#include +#include "apps/ec/radio_service/event_data.h" +#include "apps/fc/recovery_service/parachute_controller.hpp" +#include "apps/ec/primer_service/controller/primer_controller.hpp" + +#include "srp/env/EnvApp/EnvAppHandler.h" +#include "srp/env/EnvAppFc/EnvAppFcHandler.h" +#include "srp/apps/SysStatService/SysStatServiceHandler.h" +#include "srp/apps/FcSysStatService/FcSysStatServiceHandler.h" +#include "srp/apps/GPSService/GPSServiceHandler.h" +#include "srp/apps/PrimerService/PrimerServiceHandler.h" +#include "srp/apps/ServoService/ServoServiceHandler.h" +#include "srp/apps/MainService/MainServiceHandler.h" +#include "srp/apps/EngineService/EngineServiceHandler.h" +#include "srp/apps/RecoveryService/RecoveryServiceHandler.h" + +namespace srp { +namespace apps { +namespace radio { + +using RocketState_t = core::rocketState::RocketState_t; +using PrimerState_t = srp::primer::PrimerState_t; +using timepoint = std::chrono::_V2::system_clock::time_point; + +class SomeIPController { + private: + const ara::log::Logger& someip_logger; + std::shared_ptr event_data; + + PrimerServiceProxy primer_service_proxy; + std::shared_ptr primer_service_handler; + ServoServiceProxy servo_service_proxy; + std::shared_ptr servo_service_handler; + env::EnvAppProxy env_service_proxy; + std::shared_ptr env_service_handler; + srp::env::EnvAppFcProxy env_fc_service_proxy; + std::shared_ptr env_fc_service_handler; + GPSServiceProxy gps_service_proxy; + std::shared_ptr gps_service_handler; + std::shared_ptr main_service_handler; + MainServiceProxy main_service_proxy; + std::shared_ptr engine_service_handler; + EngineServiceProxy engine_service_proxy; + std::shared_ptr recovery_service_handler; + RecoveryServiceProxy recovery_service_proxy; + void SomeIpInit(); + + public: + SomeIPController(); + ~SomeIPController(); + std::shared_ptr GetMainServiceHandler(); + std::shared_ptr GetEngineServiceHandler(); + std::shared_ptr GetServoServiceHandler(); +}; + +} // namespace radio +} // namespace apps +} // namespace srp + +#endif // APPS_EC_RADIO_SERVICE_SOMEIP_CONTROLLER_HPP_ diff --git a/apps/ec/radio_service/telemetry_provider.cpp b/apps/ec/radio_service/telemetry_provider.cpp new file mode 100644 index 00000000..3eb68509 --- /dev/null +++ b/apps/ec/radio_service/telemetry_provider.cpp @@ -0,0 +1,133 @@ +/** + * @file telemetry_provider.cpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#include "apps/ec/radio_service/telemetry_provider.hpp" +#include +#include "ara/log/log.h" +namespace srp { +namespace apps { +namespace radio { + +namespace { + static constexpr auto kSystemId = 1; + static constexpr auto kComponentId = 200; + + uint8_t RocketStateToMavlinkState(const core::rocketState::RocketState_t state) { + switch (state) { + using RocketState_t = core::rocketState::RocketState_t; + case RocketState_t::INIT: return SIMBA_ROCKET_STATE_INIT; + case RocketState_t::DISARM: return SIMBA_ROCKET_STATE_DISARM; + case RocketState_t::ARM: return SIMBA_ROCKET_STATE_ARM; + case RocketState_t::LAUNCH: return SIMBA_ROCKET_STATE_LAUNCH; + case RocketState_t::FLIGHT: return SIMBA_ROCKET_STATE_FLIGHT; + case RocketState_t::APOGEE: return SIMBA_ROCKET_STATE_APOGEE; + case RocketState_t::FIRST_PARACHUTE: return SIMBA_ROCKET_STATE_DESCENT_PILOT; + case RocketState_t::SECOND_PARACHUTE: return SIMBA_ROCKET_STATE_SEC_PARACHUTE; + case RocketState_t::DROP: return SIMBA_ROCKET_STATE_DESCENT_MAIN; + case RocketState_t::ABORT: return SIMBA_ROCKET_STATE_ABORT; + case RocketState_t::CONNECTION_LOST: return SIMBA_ROCKET_STATE_CONNECTION_LOST; + case RocketState_t::TOUCHDOWN: return SIMBA_ROCKET_STATE_TOUCHDOWN; + default: return SIMBA_ROCKET_STATE_ABORT; + } + } + + uint8_t PrimerStateToMavlinState(const srp::primer::PrimerState_t state) { + switch (state) { + using PrimerState_t = srp::primer::PrimerState_t; + case PrimerState_t::kUNKNOWN: return SIMBA_PRIMER_STATE_UNKNOWN; + case PrimerState_t::kCONNECTED: return SIMBA_PRIMER_STATE_CONNECTED; + case PrimerState_t::kNOT_CONNECTED: return SIMBA_PRIMER_STATE_NOT_CONNECTED; + case PrimerState_t::kSHORT_CIRCUIT: return SIMBA_PRIMER_STATE_SHORT_CIRCUIT; + default: return SIMBA_PRIMER_STATE_UNKNOWN; + } + } +} // namespace + +std::optional TelemetryProvider::GetReqRocketStateFromGSFlags(const uint8_t flags) { + static constexpr std::pair gs_rocket_state_mapping[] = { + {SIMBA_GS_FLAGS_ABORT, RocketState_t::ABORT}, + {SIMBA_GS_FLAGS_LAUNCH, RocketState_t::LAUNCH}, + {SIMBA_GS_FLAGS_ARM, RocketState_t::ARM}, + {SIMBA_GS_FLAGS_DISARM, RocketState_t::DISARM}, + }; + for (const auto& [mask, state] : gs_rocket_state_mapping) { + if ((flags & mask) != 0) return state; + } + + return std::nullopt; +} + +TelemetryProvider::TelemetryProvider() : event_data(EventData::GetInstance()) { + if (!timestamp_.Init()) { + ara::log::LogWarn() << "TelemetryProvider: Cant initialize app time"; + } +} + +uint8_t TelemetryProvider::MapState(core::rocketState::RocketState_t s) { + return RocketStateToMavlinkState(s); +} + +uint8_t TelemetryProvider::MapPrimer(srp::primer::PrimerState_t p) { + return PrimerStateToMavlinState(p); +} + +std::optional> TelemetryProvider::GetHeartbeatMsg() { + auto ts = timestamp_.GetNewTimeStamp(); + if (!ts.has_value()) return std::nullopt; + return Pack([&](mavlink_message_t* m) { + mavlink_msg_simba_rocket_heartbeat_pack( + kSystemId, kComponentId, m, + static_cast(ts.value()), 0, + MapState(event_data->GetComputerState(BoardType_e::MB)), + MapState(event_data->GetComputerState(BoardType_e::EB)), + event_data->GetActuatorStates(), + MapPrimer(static_cast(event_data->GetPrimerStates()))); + }); +} + +std::vector TelemetryProvider::GetMaxAltitudeMsg() { + return Pack([&](mavlink_message_t* m) { + mavlink_msg_simba_max_altitude_pack(kSystemId, kComponentId, m, event_data->GetMaxAltitude()); + }); +} + +std::vector TelemetryProvider::GetTankSensorsMsg() { + return Pack([&](mavlink_message_t* m) { + mavlink_msg_simba_tank_sensors_pack(kSystemId, kComponentId, m, + static_cast(event_data->GetTemp(0)), + static_cast(event_data->GetTemp(1)), + static_cast(event_data->GetTemp(2)), + event_data->GetPress()); + }); +} + +std::vector TelemetryProvider::GetGpsMsg() { + auto gps_data = event_data->GetGPS(); + return Pack([&](mavlink_message_t* m) { + mavlink_msg_simba_gps_pack(kSystemId, kComponentId, m, gps_data.lon, gps_data.lat, gps_data.altitude); + }); +} + +std::vector TelemetryProvider::GetComputersTelemetryMsg() { + auto eb = event_data->GetComputerTelemetry(BoardType_e::EB); + auto mb = event_data->GetComputerTelemetry(BoardType_e::MB); + + return Pack([&](mavlink_message_t* m) { + mavlink_msg_simba_computers_telemetry_pack( + kSystemId, kComponentId, m, + mb.cpu_usage, mb.mem_usage, + eb.cpu_usage, eb.mem_usage, + mb.temps, eb.temps); + }); +} + +} // namespace radio +} // namespace apps +} // namespace srp diff --git a/apps/ec/radio_service/telemetry_provider.hpp b/apps/ec/radio_service/telemetry_provider.hpp new file mode 100644 index 00000000..74d4020d --- /dev/null +++ b/apps/ec/radio_service/telemetry_provider.hpp @@ -0,0 +1,66 @@ +/** + * @file telemetry_provider.hpp + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2026-04-17 + * + * @copyright Copyright (c) 2026 + * + */ +#ifndef APPS_EC_RADIO_SERVICE_TELEMETRY_PROVIDER_HPP_ +#define APPS_EC_RADIO_SERVICE_TELEMETRY_PROVIDER_HPP_ + +#include +#include +#include +#include "simba/mavlink.h" +#include "simba/simba.h" +#include "apps/ec/radio_service/event_data.h" +#include "core/timestamp/timestamp_driver.hpp" +#include "apps/ec/primer_service/controller/primer_controller.hpp" + + +namespace srp { +namespace apps { +namespace radio { +using RocketState_t = core::rocketState::RocketState_t; +using PrimerState_t = srp::primer::PrimerState_t; +using timepoint = std::chrono::_V2::system_clock::time_point; + +class TelemetryProvider { + public: + TelemetryProvider(); + + template + std::vector Pack(PackFunc&& pack_func) { + mavlink_message_t msg; + pack_func(&msg); + + std::vector buffer(MAVLINK_MAX_PACKET_LEN); + uint16_t len = mavlink_msg_to_send_buffer(buffer.data(), &msg); + + buffer.resize(len); + return buffer; + } + + std::optional> GetHeartbeatMsg(); + std::vector GetMaxAltitudeMsg(); + std::vector GetTankSensorsMsg(); + std::vector GetGpsMsg(); + std::vector GetComputersTelemetryMsg(); + + std::optional GetReqRocketStateFromGSFlags(const uint8_t flags); + + private: + std::shared_ptr event_data; + core::timestamp::TimestampController timestamp_; + + uint8_t MapState(core::rocketState::RocketState_t s); + uint8_t MapPrimer(srp::primer::PrimerState_t p); +}; + +} // namespace radio +} // namespace apps +} // namespace srp +#endif // APPS_EC_RADIO_SERVICE_TELEMETRY_PROVIDER_HPP_ diff --git a/apps/ec/radio_service/ut/BUILD b/apps/ec/radio_service/ut/BUILD new file mode 100644 index 00000000..74cdd7d9 --- /dev/null +++ b/apps/ec/radio_service/ut/BUILD @@ -0,0 +1,26 @@ +# cc_test( +# name = "radio_app_test", +# srcs = [ +# "radio_app_test.cc" +# ], +# visibility = ["//visibility:public"], +# deps = [ +# "@com_google_googletest//:gtest_main", +# "//apps/fc/radio_service:radio_app_lib", +# "//core/uart:mock_uart", +# "//core/timestamp:mock_timestamp_controller", +# ], +# ) +# TODO(matikrajek42@gmail.com) fix UT + +# cc_test( +# name = "event_data_test", +# srcs = [ +# "event_data_test.cc", +# ], +# visibility = ["//visibility:public"], +# deps = [ +# "@com_google_googletest//:gtest_main", +# "//apps/fc/radio_service:radio_app_lib", +# ], +# ) diff --git a/apps/ec/radio_service/ut/event_data_test.cc b/apps/ec/radio_service/ut/event_data_test.cc new file mode 100644 index 00000000..16a9fe43 --- /dev/null +++ b/apps/ec/radio_service/ut/event_data_test.cc @@ -0,0 +1,151 @@ +// /** +// * @file event_data_test.cc +// * @author Mateusz Krajewski (matikrajek42@gmail.com) +// * @brief +// * @version 0.1 +// * @date 2025-04-26 +// * +// * @copyright Copyright (c) 2025 +// * +// */ +// #include "gtest/gtest.h" +// #include "apps/ec/radio_service/event_data.h" +// namespace srp::apps { +// // --- Temperature Test --- +// class TemperatureTest : public ::testing::TestWithParam> { +// protected: +// std::shared_ptr data; +// void SetUp() override { data = EventData::GetInstance(); } +// }; + +// TEST_P(TemperatureTest, SetAndGetTemperature) { +// int temp_number = std::get<0>(GetParam()); +// int16_t value = std::get<1>(GetParam()); + +// switch (temp_number) { +// case 1: +// data->SetTemp1(value); +// EXPECT_EQ(data->GetTemp1(), value); +// break; +// case 2: +// data->SetTemp2(value); +// EXPECT_EQ(data->GetTemp2(), value); +// break; +// case 3: +// data->SetTemp3(value); +// EXPECT_EQ(data->GetTemp3(), value); +// break; +// default: +// FAIL() << "Invalid temperature sensor number"; +// } +// } + +// INSTANTIATE_TEST_SUITE_P( +// TemperatureTests, +// TemperatureTest, +// ::testing::Values( +// std::make_tuple(1, 0), // minimal +// std::make_tuple(2, INT16_MAX), // maksymalna wartość int16_t +// std::make_tuple(3, 100), // środek zakresu +// std::make_tuple(1, 42), // losowa wartość +// std::make_tuple(2, 1000) // wartość dodatnia +// ) +// ); + +// // --- Pressure Test --- +// class PressureTest : public ::testing::TestWithParam> { +// protected: +// std::shared_ptr data; +// void SetUp() override { data = EventData::GetInstance(); } +// }; + +// TEST_P(PressureTest, SetAndGetPressure) { +// auto [field, value] = GetParam(); + +// if (field == "Press") { +// uint16_t press_int = static_cast(value); +// data->SetPress(press_int); +// // GetPress returns float, but SetPress accepts uint16_t, so compare as uint16_t +// EXPECT_EQ(static_cast(data->GetPress()), press_int); +// } else { +// FAIL() << "Invalid pressure field"; +// } +// } + +// INSTANTIATE_TEST_SUITE_P( +// PressureTests, +// PressureTest, +// ::testing::Values( +// std::make_tuple("Press", 0.0f), // min +// std::make_tuple("Press", 65535.0f), // max uint16_t +// std::make_tuple("Press", 1000.0f), // typowa wartość +// std::make_tuple("Press", 123.456f), // losowa +// std::make_tuple("Press", 789.123f) // losowa +// ) +// ); + +// // --- GPS Test --- +// class GPSTest : public ::testing::TestWithParam> { +// protected: +// std::shared_ptr data; +// void SetUp() override { data = EventData::GetInstance(); } +// }; + +// TEST_P(GPSTest, SetAndGetGPS) { +// auto [lon, lat] = GetParam(); +// data->SetGPS(lon, lat, 0); +// EXPECT_EQ(data->GetGPSLon(), lon); +// EXPECT_EQ(data->GetGPSLat(), lat); +// } + +// INSTANTIATE_TEST_SUITE_P( +// GPSTests, +// GPSTest, +// ::testing::Values( +// std::make_tuple(INT32_MIN, INT32_MIN), // minimalne +// std::make_tuple(INT32_MAX, INT32_MAX), // maksymalne +// std::make_tuple(0, 0), // zerowe +// std::make_tuple(50000000, 25000000), // typowe współrzędne +// std::make_tuple(-100000000, 75000000) // negatywne wartości +// ) +// ); + +// // --- Actuator State Test --- +// class ActuatorStateTest : public ::testing::TestWithParam> { +// protected: +// std::shared_ptr data; +// void SetUp() override { data = EventData::GetInstance(); } +// }; + +// TEST_P(ActuatorStateTest, SetAndGetActuatorState) { +// auto [actuator_id, state] = GetParam(); +// data->SetActuatorState(actuator_id, state); +// uint8_t actuator_states = data->GetActuatorStates(); +// EXPECT_EQ(((actuator_states >> actuator_id) & 1), state); +// } + +// INSTANTIATE_TEST_SUITE_P( +// ActuatorStateTests, +// ActuatorStateTest, +// ::testing::Values( +// std::make_tuple(0, 1), // pierwszy aktuator włączony +// std::make_tuple(1, 1), // drugi aktuator włączony +// std::make_tuple(2, 1), // trzeci aktuator włączony +// std::make_tuple(0, 0), // pierwszy aktuator wyłączony +// std::make_tuple(1, 0), // drugi aktuator wyłączony +// std::make_tuple(2, 0) // trzeci aktuator wyłączony +// ) +// ); + +// TEST(EventDataSingletonTest, SameInstanceReturned) { +// auto instance1 = EventData::GetInstance(); +// auto instance2 = EventData::GetInstance(); +// EXPECT_EQ(instance1.get(), instance2.get()); +// instance1->SetTemp1(1); +// instance1->SetTemp2(2); +// instance1->SetTemp3(3); +// EXPECT_EQ(instance1->GetTemp1(), instance2->GetTemp1()); +// EXPECT_EQ(instance1->GetTemp2(), instance2->GetTemp2()); +// EXPECT_EQ(instance1->GetTemp3(), instance2->GetTemp3()); +// } +// } // namespace srp::apps diff --git a/apps/ec/radio_service/ut/radio_app_test.cc b/apps/ec/radio_service/ut/radio_app_test.cc new file mode 100644 index 00000000..6f180408 --- /dev/null +++ b/apps/ec/radio_service/ut/radio_app_test.cc @@ -0,0 +1,172 @@ +/** + * @file radio_app_test.cc + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-23 + * + * @copyright Copyright (c) 2025 + * + */ +#include +#include "apps/ec/radio_service/radio_app.h" +#include "core/uart/mock_uart_driver.hpp" +#include "core/timestamp/mock_timestamp_driver.h" + +using ::testing::_; +using ::testing::Invoke; + +TEST(RADIOAPPTEST, InitializeTestNoUart) { + ara::log::LoggingMenager::Create("123", ara::log::LogMode::kConsole); + const std::map map; + srp::apps::RadioApp app; + EXPECT_EQ(app.Initialize(map), 1); +} + +// TEST(RADIOAPPTEST, InitializeTestMock) { +// auto mock_uart = std::make_unique(); +// EXPECT_CALL(*mock_uart, Open(::testing::_, ::testing::_)) +// .Times(1) +// .WillOnce(::testing::Return(true)); +// const std::map map; +// srp::apps::RadioApp app; +// app.InitUart(std::move(mock_uart)); +// EXPECT_EQ(app.Initialize(map), 0); +// } + +class TestWrapper : public srp::apps::RadioApp { + public: + TestWrapper(): RadioApp() { + this->event_data = srp::apps::EventData::GetInstance(); + } + void TestInitUart(std::unique_ptr uart) { + InitUart(std::move(uart)); + } + void TestInitTimestamp(std::unique_ptr timestamp) { + InitTimestamp(std::move(timestamp)); + } + void TestTransmittingLoop(const std::stop_token& token) { + this->TransmittingLoop(token); + } + std::shared_ptr GetEventDataPtr() { + return this->event_data; + } +}; + +struct EventDataTestParams { + uint16_t input_temp1; + uint16_t input_temp2; + uint16_t input_temp3; + float input_pressure; + int32_t input_gpslat; + int32_t input_gpslon; + std::optional input_timestamp; + uint8_t input_primer; + uint8_t input_servo; + uint8_t input_servovent; +}; + +class EventDataTest : public ::testing::TestWithParam {}; + +// // INSTANTIATE_TEST_SUITE_P - Tests that radio app correctly sends messages +// INSTANTIATE_TEST_SUITE_P(EventDataTestParameters, EventDataTest, +// ::testing::Values( +// EventDataTestParams{ +// 0, 0, 0, 0.0f, 0, 0, +// std::optional{0}, +// 0, 0, 0 +// }, +// EventDataTestParams{ +// 0xFFFF, 0xFFFF, 0xFFFF, FLT_MAX, 0x7FFFFFFF, 0x7FFFFFFF, +// std::optional{INT64_MAX}, +// 1, 1, 1 +// } +// ) +// ); + +// TEST_P(EventDataTest, ValidateEventData) { +// auto params = GetParam(); +// TestWrapper wrapper_; +// auto mock_uart = std::make_unique(); +// std::vector> sent_data; +// EXPECT_CALL(*mock_uart, Write(_)) +// .Times(5) +// .WillRepeatedly(Invoke([&sent_data](const std::vector& data) -> srp::core::ErrorCode { +// sent_data.push_back(data); +// return srp::core::ErrorCode::kOk; +// })); +// auto mock_timestamp = std::make_unique(); +// EXPECT_CALL(*mock_timestamp, GetNewTimeStamp()) +// .Times(1) +// .WillOnce(::testing::Return(params.input_timestamp)); +// EXPECT_CALL(*mock_timestamp, Init()) +// .Times(1); +// wrapper_.TestInitUart(std::move(mock_uart)); +// wrapper_.TestInitTimestamp(std::move(mock_timestamp)); +// auto event_data = srp::apps::EventData::GetInstance(); + +// // Reset all values to ensure clean state +// event_data->SetTemp1(0); +// event_data->SetTemp2(0); +// event_data->SetTemp3(0); +// event_data->SetPress(0); +// event_data->SetGPS(0, 0, 0); +// event_data->SetActuatorState(0, 0); +// event_data->SetActuatorState(1, 0); +// event_data->SetActuatorState(2, 0); + +// // Set test values +// event_data->SetTemp1(static_cast(params.input_temp1)); +// event_data->SetTemp2(static_cast(params.input_temp2)); +// event_data->SetTemp3(static_cast(params.input_temp3)); +// event_data->SetPress(static_cast(params.input_pressure)); +// event_data->SetGPS(params.input_gpslon, params.input_gpslat, 0); +// event_data->SetActuatorState(0, params.input_primer); +// event_data->SetActuatorState(1, params.input_servo); +// event_data->SetActuatorState(2, params.input_servovent); + +// std::jthread transmitting_thread([&wrapper_](const std::stop_token& token) { +// wrapper_.TestTransmittingLoop(token); +// }); +// ASSERT_EQ(event_data.get(), wrapper_.GetEventDataPtr().get()); +// std::this_thread::sleep_for(std::chrono::milliseconds(900)); +// transmitting_thread.request_stop(); +// transmitting_thread.join(); +// EXPECT_EQ(sent_data.size(), 5); + +// // Verify message structure: {STX, payloadLen, seq, systemId, compId, msgId, ... payload ..., CRC1, CRC2} +// // Message order: heartbeat, max_altitude, tank_temperature, tank_pressure, gps +// const uint8_t kFrameStart = 254; +// const uint8_t kSystemId = 1; +// const uint8_t kComponentId = 200; + +// // Check heartbeat (msgId=73) +// EXPECT_EQ(sent_data[0][0], kFrameStart); +// EXPECT_EQ(sent_data[0][3], kSystemId); +// EXPECT_EQ(sent_data[0][4], kComponentId); +// EXPECT_EQ(sent_data[0][5], 73); // SIMBA_ROCKET_HEARTBEAT + +// // Check max_altitude (msgId=75) +// EXPECT_EQ(sent_data[1][0], kFrameStart); +// EXPECT_EQ(sent_data[1][3], kSystemId); +// EXPECT_EQ(sent_data[1][4], kComponentId); +// EXPECT_EQ(sent_data[1][5], 75); // SIMBA_MAX_ALTITUDE + +// // Check tank_temperature (msgId=69) +// EXPECT_EQ(sent_data[2][0], kFrameStart); +// EXPECT_EQ(sent_data[2][3], kSystemId); +// EXPECT_EQ(sent_data[2][4], kComponentId); +// EXPECT_EQ(sent_data[2][5], 69); // SIMBA_TANK_TEMPERATURE + +// // Check tank_pressure (msgId=70) +// EXPECT_EQ(sent_data[3][0], kFrameStart); +// EXPECT_EQ(sent_data[3][3], kSystemId); +// EXPECT_EQ(sent_data[3][4], kComponentId); +// EXPECT_EQ(sent_data[3][5], 70); // SIMBA_TANK_PRESSURE + +// // Check GPS (msgId=72) +// EXPECT_EQ(sent_data[4][0], kFrameStart); +// EXPECT_EQ(sent_data[4][3], kSystemId); +// EXPECT_EQ(sent_data[4][4], kComponentId); +// EXPECT_EQ(sent_data[4][5], 72); // SIMBA_GPS +// } diff --git a/apps/fc/main_service/BUILD b/apps/fc/main_service/BUILD index 8045105a..6805a0be 100644 --- a/apps/fc/main_service/BUILD +++ b/apps/fc/main_service/BUILD @@ -9,7 +9,8 @@ cc_binary( visibility = [ "//deployment/apps/fc/main_app:__subpackages__", "//apps/fc/radio_service:__subpackages__", - ], + "//apps/ec/radio_service:__subpackages__", + ], deps = [ "@srp_platform//ara/exec:adaptive_application_lib", "@srp_platform//ara/diag:uds_lib", diff --git a/deployment/apps/engine_app/BUILD b/deployment/apps/engine_app/BUILD index a62c148b..d9007ad9 100644 --- a/deployment/apps/engine_app/BUILD +++ b/deployment/apps/engine_app/BUILD @@ -15,6 +15,7 @@ filegroup( "//apps/engine_service:__subpackages__", "//deployment/apps/fc/radio_app:__subpackages__", "//deployment/apps/logger_service:__subpackages__", + "//deployment/apps/radio_app:__subpackages__", ], ) diff --git a/deployment/apps/env_service/BUILD b/deployment/apps/env_service/BUILD index 3f6e4f3b..8b34cc41 100644 --- a/deployment/apps/env_service/BUILD +++ b/deployment/apps/env_service/BUILD @@ -22,7 +22,9 @@ filegroup( "//apps/ec:__subpackages__", "//apps/env_service:__pkg__", "//deployment/apps/logger_service:__subpackages__", - "//deployment/apps/fc/radio_app:__subpackages__",], + "//deployment/apps/fc/radio_app:__subpackages__", + "//deployment/apps/radio_app:__subpackages__", + ], ) adaptive_application( diff --git a/deployment/apps/fc/env_service/BUILD b/deployment/apps/fc/env_service/BUILD index 6155d5cf..10d058ce 100644 --- a/deployment/apps/fc/env_service/BUILD +++ b/deployment/apps/fc/env_service/BUILD @@ -23,6 +23,7 @@ filegroup( "//apps/fc/env_service:__pkg__", "//deployment/apps/logger_service:__subpackages__", "//deployment/apps/fc/radio_app:__subpackages__", + "//deployment/apps/radio_app:__subpackages__", "//deployment/apps/fc/logger_service:__subpackages__", ], ) diff --git a/deployment/apps/fc/gps_app/BUILD b/deployment/apps/fc/gps_app/BUILD index 56fcc309..522d23fb 100644 --- a/deployment/apps/fc/gps_app/BUILD +++ b/deployment/apps/fc/gps_app/BUILD @@ -10,7 +10,8 @@ filegroup( visibility = [ "//apps/fc/gps_service:__subpackages__", "//deployment/apps/fc/gps_service:__subpackages__", - "//deployment/apps/fc/radio_app:__subpackages__" + "//deployment/apps/fc/radio_app:__subpackages__", + "//deployment/apps/radio_app:__subpackages__", ], ) diff --git a/deployment/apps/fc/main_app/BUILD b/deployment/apps/fc/main_app/BUILD index 48a57306..8cf6db07 100644 --- a/deployment/apps/fc/main_app/BUILD +++ b/deployment/apps/fc/main_app/BUILD @@ -13,6 +13,7 @@ filegroup( "//apps/fc/main_service:__subpackages__", "//deployment/apps/fc/main_service:__subpackages__", "//deployment/apps/fc/radio_app:__subpackages__", + "//deployment/apps/radio_app:__subpackages__", "//deployment/apps/engine_app:__subpackages__", ], ) diff --git a/deployment/apps/fc/recovery_service/BUILD b/deployment/apps/fc/recovery_service/BUILD index a0b3fe76..f1202304 100644 --- a/deployment/apps/fc/recovery_service/BUILD +++ b/deployment/apps/fc/recovery_service/BUILD @@ -13,6 +13,7 @@ filegroup( "//deployment/apps/fc/recovery_service:__subpackages__", "//deployment/apps/fc/radio_app:__subpackages__", "//deployment/apps/fc/main_app:__subpackages__", + "//deployment/apps/radio_app:__subpackages__", ], ) diff --git a/deployment/apps/prim_service/BUILD b/deployment/apps/prim_service/BUILD index 91fffd91..73cc557d 100644 --- a/deployment/apps/prim_service/BUILD +++ b/deployment/apps/prim_service/BUILD @@ -13,6 +13,7 @@ filegroup( "//deployment/apps/engine_app:__subpackages__", "//deployment/apps/fc/radio_app:__subpackages__", "//deployment/apps/logger_service:__subpackages__", + "//deployment/apps/radio_app:__subpackages__", ], ) diff --git a/deployment/apps/radio_app/BUILD b/deployment/apps/radio_app/BUILD new file mode 100644 index 00000000..83fd3589 --- /dev/null +++ b/deployment/apps/radio_app/BUILD @@ -0,0 +1,47 @@ +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/radio_service:service_someipy", + "//deployment/apps/env_service:instance", + "//deployment/apps/fc/gps_app:instance", + "//deployment/apps/prim_service:instance", + "//deployment/apps/engine_app:instance", + "//deployment/apps/servo_service:instance", + "//deployment/apps/fc/recovery_service:instance", + "//deployment/apps/fc/main_app:instance", + "//deployment/apps/fc/env_service:instance", + "//deployment/apps/system_stat_service:instance", + "//deployment/apps/fc/system_stat_service:instance", + + ], + visibility = [ + "//apps/ec/radio_service:__subpackages__", + ], +) + +ara_someip_lib( + name = "someip_lib", + model_src = ["//deployment/apps/radio_app:instance"], + visibility = ["//apps/ec/radio_service:__subpackages__"], +) + +ara_runtime_lib( + name = "ara", + model_src = ["//deployment/apps/radio_app:instance"], + visibility = ["//apps/ec/radio_service:__subpackages__"], +) + +adaptive_application( + name = "RadioApp", + bin = "//apps/ec/radio_service:radio_service", + model_src = ["//deployment/apps/radio_app:instance"], + visibility = [ + "//deployment/cpu/fc:__subpackages__", + "//deployment/cpu/ec:__subpackages__" + ], +) + diff --git a/deployment/apps/radio_app/app_config.json b/deployment/apps/radio_app/app_config.json new file mode 100644 index 00000000..11d31d29 --- /dev/null +++ b/deployment/apps/radio_app/app_config.json @@ -0,0 +1,135 @@ +{ + "include": [ + "deployment/system_definition/someip/radio_service/service.json", + "deployment/system_definition/someip/env_service/service.json", + "deployment/system_definition/someip/fc/gps_service/service.json", + "deployment/system_definition/someip/prim_service/service.json", + "deployment/system_definition/someip/engine_service/service.json", + "deployment/system_definition/someip/servo_service/service.json", + "deployment/system_definition/someip/fc/recovery_service/service.json", + "deployment/system_definition/someip/fc/main_service/service.json", + "deployment/system_definition/someip/fc/env_service/service.json", + "deployment/system_definition/someip/sys_stat_service/service.json", + "deployment/system_definition/someip/fc/sys_stat_service/service.json" + ], + "package": "srp.apps", + "adaptive_application": { + "RadioApp": { + "app": { + "functional_groups":[ + "Running", + "SafetyMode" + ], + "parms": "", + "logger": { + "app_id": "RAD-", + "app_des": "czytanie i wysyłanie po radiu", + "log_level": "kDebug", + "log_mode": "kRemote", + "ctx": [ + { + "ctx_id": "SOME", + "log_level": "kInfo", + "ctx_des": "Logs From someip" + }, + { + "ctx_id": "MAVL", + "log_level": "kInfo", + "ctx_des": "Logs From mavlink" + }, + { + "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": "kInfo", + "ctx_des": "Default ctx for ara::exec" + } + ] + } + }, + "provide": [ + { + "name": "FcRadioService as RadioService_ipc", + "on": "ipc", + "instance": 2 + }, + { + "name": "FcRadioService as RadioService_udp", + "on": "udp", + "port": "10001", + "instance": 1 + } + ], + "require": [ + { + "name": "srp.env.EnvApp as EnvApp", + "on": "udp", + "port": "10001", + "instance": 1 + }, + { + "name": "srp.apps.GPSService as GPSService", + "on": "ipc", + "instance": 2 + }, + { + "name": "srp.apps.PrimerService as PrimerService", + "on": "udp", + "port": "10002", + "instance": 1 + }, + { + "name": "srp.apps.ServoService as ServoService", + "on": "udp", + "port": "10001", + "instance": 1 + }, + { + "name": "srp.apps.RecoveryService as RecoveryService", + "on": "ipc", + "instance": 2 + }, + { + "name": "srp.apps.MainService as MainService", + "on": "ipc", + "instance": 2 + }, + { + "name": "srp.apps.EngineService as EngineService", + "on": "udp", + "port": "10001", + "instance": 1 + }, + { + "name": "srp.env.EnvAppFc as EnvAppFc", + "on": "ipc", + "instance": 2 + }, + { + "name": "SysStatService as SysStatService", + "on": "udp", + "port": "10001", + "instance": 1 + }, + { + "name": "FcSysStatService as FcSysStatService", + "on": "ipc", + "instance": 2 + } + ] + } + } +} diff --git a/deployment/apps/servo_service/BUILD b/deployment/apps/servo_service/BUILD index 88b231c3..f737c063 100644 --- a/deployment/apps/servo_service/BUILD +++ b/deployment/apps/servo_service/BUILD @@ -18,7 +18,8 @@ filegroup( "//mw/i2c_service:__subpackages__", "//deployment/apps/fc/radio_app:__subpackages__", "//deployment/apps/logger_service:__subpackages__", - ], + "//deployment/apps/radio_app:__subpackages__", + ], ) ara_someip_lib( diff --git a/deployment/cpu/ec/BUILD b/deployment/cpu/ec/BUILD index 3e950d73..95db72c0 100644 --- a/deployment/cpu/ec/BUILD +++ b/deployment/cpu/ec/BUILD @@ -26,6 +26,7 @@ cpu_def( "//deployment/apps/logger_service:FileLoggerApp", "//deployment/apps/system_stat_service:SysStatService", "//deployment/apps/prim_service:PrimerService", + "//deployment/apps/radio_app:RadioApp" ], ) diff --git a/deployment/system_definition b/deployment/system_definition index 2d29a1ee..8bd4448e 160000 --- a/deployment/system_definition +++ b/deployment/system_definition @@ -1 +1 @@ -Subproject commit 2d29a1eeeebd12915740b7783e2964168c1ff648 +Subproject commit 8bd4448e84dd2303a610218f65b293184a630530