From a67736ec54694157f3c95e68b3dacf33c05b33e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Ma=C5=84kowski?= Date: Sun, 13 Apr 2025 20:27:53 +0200 Subject: [PATCH 1/5] Basic event sending throug mavlink system definition changed system_definition changed again cpplint fixes --- WORKSPACE | 2 +- apps/fc/radio_service/BUILD | 31 +++ apps/fc/radio_service/main.cc | 17 ++ apps/fc/radio_service/radio_app.cc | 228 +++++++++++++++++++ apps/fc/radio_service/radio_app.h | 66 ++++++ deployment/apps/env_service/BUILD | 3 +- deployment/apps/fc/gps_app/BUILD | 1 + deployment/apps/fc/radio_app/BUILD | 39 ++++ deployment/apps/fc/radio_app/app_config.json | 90 ++++++++ deployment/apps/fc/recovery_service/BUILD | 1 + deployment/apps/prim_service/BUILD | 1 + deployment/apps/servo_service/BUILD | 1 + deployment/system_definition | 2 +- 13 files changed, 479 insertions(+), 3 deletions(-) create mode 100644 apps/fc/radio_service/BUILD create mode 100644 apps/fc/radio_service/main.cc create mode 100644 apps/fc/radio_service/radio_app.cc create mode 100644 apps/fc/radio_service/radio_app.h create mode 100644 deployment/apps/fc/radio_app/BUILD create mode 100644 deployment/apps/fc/radio_app/app_config.json diff --git a/WORKSPACE b/WORKSPACE index bb009028..acfee07e 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -21,7 +21,7 @@ install_python() load("@srp_platform//:pip_install.bzl", "pip_install") pip_install() -include_srp_mavlink("0.2") +include_srp_mavlink("0.3") include_gtest_mock() include_json("3.11.3") diff --git a/apps/fc/radio_service/BUILD b/apps/fc/radio_service/BUILD new file mode 100644 index 00000000..fe67de81 --- /dev/null +++ b/apps/fc/radio_service/BUILD @@ -0,0 +1,31 @@ +cc_library( + name = "radio_app_lib", + deps = [ + "@srp_platform//ara/exec:adaptive_application_lib", + "@srp_mavlink//lib:mavlink_lib", + "//deployment/apps/fc/radio_app:someip_lib", + "//deployment/apps/fc/radio_app:ara", + "//core/timestamp:timestamp_controller", + "//core/uart:uart_driver" + ], + srcs = [ + "radio_app.cc" + ], + hdrs = [ + "radio_app.h" + ], + visibility = ["//apps/fc/radio_service:__subpackages__",], +) + +cc_binary( + name = "radio_service", + srcs = [ + "main.cc", + ], + visibility = [ + "//deployment:__subpackages__", + ], + deps = [ + "//apps/fc/radio_service:radio_app_lib", + ], +) diff --git a/apps/fc/radio_service/main.cc b/apps/fc/radio_service/main.cc new file mode 100644 index 00000000..de1b0b3c --- /dev/null +++ b/apps/fc/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/fc/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/fc/radio_service/radio_app.cc b/apps/fc/radio_service/radio_app.cc new file mode 100644 index 00000000..d9029aac --- /dev/null +++ b/apps/fc/radio_service/radio_app.cc @@ -0,0 +1,228 @@ +/** + * @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 +#include +#include "apps/fc/radio_service/radio_app.h" +#include "core/common/condition.h" + +namespace srp { +namespace apps { +namespace { +constexpr auto kService_ipc_instance = "srp/apps/RadioApp/RadioService_ipc"; +constexpr auto kService_udp_instance = "srp/apps/RadioApp/RadioService_udp"; +constexpr auto kEnv_service_path_name = "srp/apps/RadioApp/EnvApp"; +constexpr auto kGPS_service_path_name = "srp/apps/RadioApp/GPSService"; +constexpr auto kPrimer_service_path_name = "srp/apps/RadioApp/PrimerService"; +constexpr auto kServo_service_path_name = "srp/apps/RadioApp/ServoService"; +constexpr auto kRecovery_service_path_name = "srp/apps/RadioApp/RecoveryService"; +constexpr auto KGPS_UART_path = "/dev/ttyS1"; +constexpr auto KGPS_UART_baudrate = B115200; +constexpr auto systemId = 1; +} // namespace + + + +void RadioApp::TransmittingLoop(const std::stop_token& token) { + core::timestamp::TimestampController timestamp_; + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + timestamp_.Init(); + while (!token.stop_requested()) { + // TODO(m.mankowski2004@gmail.com): Change the component IDs + std::unique_lock lock(this->mutex_); + mavlink_msg_simba_tank_temperature_pack(systemId, 200, &msg, + temp.temp1, temp.temp2, temp.temp3); + uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); + uart_->Write(std::vector(buffer, buffer + len)); + + mavlink_msg_simba_tank_pressure_pack(systemId, 200, &msg, + press.Dpressure, press.pressure); + len = mavlink_msg_to_send_buffer(buffer, &msg); + uart_->Write(std::vector(buffer, buffer + len)); + + mavlink_msg_simba_gps_pack(systemId, 200, &msg, + gps.lon, gps.lat, 0); // add altitude later + len = mavlink_msg_to_send_buffer(buffer, &msg); + uart_->Write(std::vector(buffer, buffer + len)); + + auto val = timestamp_.GetNewTimeStamp(); + if (val.has_value()) { + mavlink_msg_simba_heartbeat_pack(systemId, 200, &msg, + static_cast(val.value()), 0, 0); // add later the status of both computers + len = mavlink_msg_to_send_buffer(buffer, &msg); + uart_->Write(std::vector(buffer, buffer + len)); + } + mavlink_msg_simba_actuator_pack(systemId, 200, &msg, + actuator.values); + len = mavlink_msg_to_send_buffer(buffer, &msg); + uart_->Write(std::vector(buffer, buffer + len)); + lock.unlock(); + core::condition::wait_for(std::chrono::milliseconds(1000), token); // 1 Hz + } +} + +int RadioApp::Run(const std::stop_token& token) { + std::jthread transmitting_thread([this](const std::stop_token& t) { + this->TransmittingLoop(t); + }); + core::condition::wait(token); + service_ipc->StopOffer(); + service_udp->StopOffer(); + uart_->Close(); + return core::ErrorCode::kOk; +} + +void RadioApp::Init(std::unique_ptr uart) { + this->uart_ = std::move(uart); +} +int RadioApp::Initialize(const std::map parms) { + if (!this->uart_) { + auto uart_d = std::make_unique(); + Init(std::move(uart_d)); + } + if (!this->uart_->Open(KGPS_UART_path, KGPS_UART_baudrate)) { + return 1; + } + this->service_ipc = std::make_unique + (this->service_ipc_instance); + this->service_udp = std::make_unique + (this->service_udp_instance); + service_ipc->StartOffer(); + service_udp->StartOffer(); + this->SomeIpInit(); + return core::ErrorCode::kOk; +} +void RadioApp::SomeIpInit() { + this->env_service_proxy.StartFindService([this](auto handler) { + this->env_service_handler = handler; + env_service_handler->newTempEvent_1.Subscribe(1, [this](const uint8_t status) { + env_service_handler->newTempEvent_1.SetReceiveHandler([this] () { + auto res = env_service_handler->newTempEvent_1.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + this->temp.temp1 = res.Value(); + }); + }); + env_service_handler->newTempEvent_2.Subscribe(1, [this](const uint8_t status) { + env_service_handler->newTempEvent_2.SetReceiveHandler([this] () { + auto res = env_service_handler->newTempEvent_2.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + this->temp.temp2 = res.Value(); + }); + }); + env_service_handler->newTempEvent_3.Subscribe(1, [this](const uint8_t status) { + env_service_handler->newTempEvent_3.SetReceiveHandler([this] () { + auto res = env_service_handler->newTempEvent_3.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + this->temp.temp3 = res.Value(); + }); + }); + env_service_handler->newDPressEvent.Subscribe(1, [this](const uint8_t status) { + env_service_handler->newDPressEvent.SetReceiveHandler([this] () { + auto res = env_service_handler->newDPressEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + this->press.Dpressure = res.Value(); + }); + }); + env_service_handler->newPressEvent.Subscribe(1, [this](const uint8_t status) { + env_service_handler->newPressEvent.SetReceiveHandler([this] () { + auto res = env_service_handler->newPressEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + this->press.pressure = res.Value(); + }); + }); + }); + this->gps_service_proxy.StartFindService([this](auto handler) { + this->gps_service_handler = handler; + gps_service_handler->GPSStatusEvent.Subscribe(1, [this](const uint8_t status) { + gps_service_handler->GPSStatusEvent.SetReceiveHandler([this] () { + auto res = gps_service_handler->GPSStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + std::memcpy(&this->gps.lon, &res.Value().longitude, sizeof(float)); + std::memcpy(&this->gps.lat, &res.Value().latitude, sizeof(float)); + }); + }); + }); + this->primer_service_proxy.StartFindService([this](auto handler) { + this->primer_service_handler = handler; + primer_service_handler->primeStatusEvent.Subscribe(1, [this](const uint8_t status) { + primer_service_handler->primeStatusEvent.SetReceiveHandler([this] () { + auto res = primer_service_handler->primeStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + uint8_t bit_position = 0; + this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res.Value() << bit_position); + }); + }); + }); + this->servo_service_proxy.StartFindService([this](auto handler) { + this->servo_service_handler = handler; + servo_service_handler->ServoStatusEvent.Subscribe(1, [this](const uint8_t status) { + servo_service_handler->ServoStatusEvent.SetReceiveHandler([this] () { + auto res = servo_service_handler->ServoStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + uint8_t bit_position = 1; + this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res.Value() << bit_position); + }); + }); + servo_service_handler->ServoVentStatusEvent.Subscribe(1, [this](const uint8_t status) { + servo_service_handler->ServoVentStatusEvent.SetReceiveHandler([this] () { + auto res = servo_service_handler->ServoVentStatusEvent.GetNewSamples(); + if (!res.HasValue()) { + return; + } + std::unique_lock lock(this->mutex_); + uint8_t bit_position = 2; + this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res.Value() << bit_position); + }); + }); + }); + // TODO(m.mankowski2004@gmail.com): Finish actuators + } +RadioApp::~RadioApp() { +} +RadioApp::RadioApp(): service_ipc_instance(kService_ipc_instance), + service_udp_instance(kService_udp_instance), +env_service_proxy{ara::core::InstanceSpecifier{kEnv_service_path_name}}, +env_service_handler{nullptr}, +gps_service_proxy{ara::core::InstanceSpecifier{kGPS_service_path_name}}, +gps_service_handler{nullptr}, +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} { +} + +} // namespace apps +} // namespace srp diff --git a/apps/fc/radio_service/radio_app.h b/apps/fc/radio_service/radio_app.h new file mode 100644 index 00000000..4be4074d --- /dev/null +++ b/apps/fc/radio_service/radio_app.h @@ -0,0 +1,66 @@ +/** + * @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_FC_RADIO_SERVICE_RADIO_APP_H_ +#define APPS_FC_RADIO_SERVICE_RADIO_APP_H_ + +#include +#include // NOLINT +#include + +#include "ara/exec/adaptive_application.h" +#include "lib/simba/mavlink.h" +#include "srp/env/EnvApp/EnvAppHandler.h" +#include "srp/apps/GPSService/GPSServiceHandler.h" +#include "srp/apps/PrimerService/PrimerServiceHandler.h" +#include "srp/apps/ServoService/ServoServiceHandler.h" +#include "core/timestamp/timestamp_driver.hpp" +#include "srp/apps/RadioServiceSkeleton.h" +#include "core/uart/uart_driver.hpp" + + +namespace srp { +namespace apps { +class RadioApp final : public ara::exec::AdaptiveApplication { + private: + __mavlink_simba_tank_temperature_t temp; + __mavlink_simba_tank_pressure_t press; + __mavlink_simba_gps_t gps; + __mavlink_simba_actuator_t actuator; + std::mutex mutex_; + 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; + GPSServiceProxy gps_service_proxy; + std::shared_ptr gps_service_handler; + const ara::core::InstanceSpecifier service_ipc_instance; + const ara::core::InstanceSpecifier service_udp_instance; + std::unique_ptr service_ipc; + std::unique_ptr service_udp; + std::unique_ptr uart_; + void SomeIpInit(); + void TransmittingLoop(const std::stop_token& token); + + public: + int Run(const std::stop_token& token) override; + void Init(std::unique_ptr uart); + int Initialize(const std::map + parms) override; + ~RadioApp(); + RadioApp(); +}; +} // namespace apps +} // namespace srp + +#endif // APPS_FC_RADIO_SERVICE_RADIO_APP_H_ diff --git a/deployment/apps/env_service/BUILD b/deployment/apps/env_service/BUILD index eb4ff812..63e04a6d 100644 --- a/deployment/apps/env_service/BUILD +++ b/deployment/apps/env_service/BUILD @@ -21,7 +21,8 @@ filegroup( visibility = [ "//apps/ec:__subpackages__", "//apps/env_service:__pkg__", - "//deployment/apps/logger_service:__subpackages__"], + "//deployment/apps/logger_service:__subpackages__", + "//deployment/apps/fc/radio_app:__subpackages__",], ) adaptive_application( diff --git a/deployment/apps/fc/gps_app/BUILD b/deployment/apps/fc/gps_app/BUILD index 6c08bc5a..56fcc309 100644 --- a/deployment/apps/fc/gps_app/BUILD +++ b/deployment/apps/fc/gps_app/BUILD @@ -10,6 +10,7 @@ filegroup( visibility = [ "//apps/fc/gps_service:__subpackages__", "//deployment/apps/fc/gps_service:__subpackages__", + "//deployment/apps/fc/radio_app:__subpackages__" ], ) diff --git a/deployment/apps/fc/radio_app/BUILD b/deployment/apps/fc/radio_app/BUILD new file mode 100644 index 00000000..582c882c --- /dev/null +++ b/deployment/apps/fc/radio_app/BUILD @@ -0,0 +1,39 @@ +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/radio_service:service_someipy", + "//deployment/apps/env_service:instance", + "//deployment/apps/fc/gps_app:instance", + "//deployment/apps/prim_service:instance", + "//deployment/apps/servo_service:instance", + "//deployment/apps/fc/recovery_service:instance" + ], + visibility = [ + "//apps/fc/radio_service:__subpackages__", + "//deployment/apps/fc/radio_service:__subpackages__", + ], +) + +ara_someip_lib( + name = "someip_lib", + model_src = ["//deployment/apps/fc/radio_app:instance"], + visibility = ["//apps/fc/radio_service:__subpackages__"], +) + +ara_runtime_lib( + name = "ara", + model_src = ["//deployment/apps/fc/radio_app:instance"], + visibility = ["//apps/fc/radio_service:__subpackages__"], +) + +adaptive_application( + name = "RadioApp", + bin = "//apps/fc/radio_service", + model_src = ["//deployment/apps/fc/radio_app:instance"], + visibility = ["//deployment/cpu/fc:__subpackages__"], +) + diff --git a/deployment/apps/fc/radio_app/app_config.json b/deployment/apps/fc/radio_app/app_config.json new file mode 100644 index 00000000..34a3a527 --- /dev/null +++ b/deployment/apps/fc/radio_app/app_config.json @@ -0,0 +1,90 @@ +{ + "include": [ + "deployment/system_definition/someip/fc/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/servo_service/service.json", + "deployment/system_definition/someip/fc/recovery_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": "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": "RadioService as RadioService_ipc", + "on": "ipc", + "instance": 2 + }, + { + "name": "RadioService as RadioService_udp", + "on": "udp", + "port": "10002", + "instance": 1 + } + ], + "require": [ + { + "name": "srp.env.EnvApp as EnvApp", + "on": "ipc", + "instance": 2 + }, + { + "name": "srp.apps.GPSService as GPSService", + "on": "ipc", + "instance": 2 + }, + { + "name": "srp.apps.PrimerService as PrimerService", + "on": "ipc", + "instance": 2 + }, + { + "name": "srp.apps.ServoService as ServoService", + "on": "ipc", + "instance": 2 + }, + { + "name": "srp.apps.RecoveryService as RecoveryService", + "on": "ipc", + "instance": 2 + } + ] + } + } +} \ No newline at end of file diff --git a/deployment/apps/fc/recovery_service/BUILD b/deployment/apps/fc/recovery_service/BUILD index e27afda9..17f91c95 100644 --- a/deployment/apps/fc/recovery_service/BUILD +++ b/deployment/apps/fc/recovery_service/BUILD @@ -11,6 +11,7 @@ filegroup( visibility = [ "//apps/fc/recovery_service:__subpackages__", "//deployment/apps/fc/recovery_service:__subpackages__", + "//deployment/apps/fc/radio_app:__subpackages__" ], ) diff --git a/deployment/apps/prim_service/BUILD b/deployment/apps/prim_service/BUILD index dcd59e95..f4298afd 100644 --- a/deployment/apps/prim_service/BUILD +++ b/deployment/apps/prim_service/BUILD @@ -10,6 +10,7 @@ filegroup( visibility = [ "//apps/prim_service:__subpackages__", "//deployment/apps/engine_app:__subpackages__", + "//deployment/apps/fc/radio_app:__subpackages__" ], ) diff --git a/deployment/apps/servo_service/BUILD b/deployment/apps/servo_service/BUILD index c55a0178..71fd991f 100644 --- a/deployment/apps/servo_service/BUILD +++ b/deployment/apps/servo_service/BUILD @@ -15,6 +15,7 @@ filegroup( "//deployment/apps/servo_service:__subpackages__", "//deployment/apps/engine_app:__subpackages__", "//mw/i2c_service:__subpackages__", + "//deployment/apps/fc/radio_app:__subpackages__" ], ) diff --git a/deployment/system_definition b/deployment/system_definition index b1cfb173..ef2086de 160000 --- a/deployment/system_definition +++ b/deployment/system_definition @@ -1 +1 @@ -Subproject commit b1cfb173709e9149c96a4e644564e9eed04e1ca2 +Subproject commit ef2086def1d0e95182e5c788e89a0394c540b75a From 6041d3d15d73498a42da0095e0585c3f5e7489d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Ma=C5=84kowski?= Date: Fri, 25 Apr 2025 21:16:28 +0200 Subject: [PATCH 2/5] partially done ut --- apps/fc/radio_service/BUILD | 3 +- apps/fc/radio_service/event_data.h | 126 ++++++++++++++++ apps/fc/radio_service/radio_app.cc | 107 +++++++------ apps/fc/radio_service/radio_app.h | 15 +- apps/fc/radio_service/ut/BUILD | 13 ++ apps/fc/radio_service/ut/radio_app_test.cc | 151 +++++++++++++++++++ core/timestamp/BUILD | 16 +- core/timestamp/Itimestamp_driver.h | 40 +++++ core/timestamp/mock_timestamp_driver.h | 31 ++++ core/timestamp/timestamp_driver.hpp | 15 +- deployment/apps/fc/radio_app/app_config.json | 2 +- deployment/apps/fc/recovery_service/BUILD | 2 +- deployment/apps/prim_service/BUILD | 2 +- deployment/apps/servo_service/BUILD | 2 +- deployment/cpu/fc/BUILD | 1 + 15 files changed, 451 insertions(+), 75 deletions(-) create mode 100644 apps/fc/radio_service/event_data.h create mode 100644 apps/fc/radio_service/ut/BUILD create mode 100644 apps/fc/radio_service/ut/radio_app_test.cc create mode 100644 core/timestamp/Itimestamp_driver.h create mode 100644 core/timestamp/mock_timestamp_driver.h diff --git a/apps/fc/radio_service/BUILD b/apps/fc/radio_service/BUILD index fe67de81..bee4aa41 100644 --- a/apps/fc/radio_service/BUILD +++ b/apps/fc/radio_service/BUILD @@ -12,7 +12,8 @@ cc_library( "radio_app.cc" ], hdrs = [ - "radio_app.h" + "radio_app.h", + "event_data.h", ], visibility = ["//apps/fc/radio_service:__subpackages__",], ) diff --git a/apps/fc/radio_service/event_data.h b/apps/fc/radio_service/event_data.h new file mode 100644 index 00000000..02fae227 --- /dev/null +++ b/apps/fc/radio_service/event_data.h @@ -0,0 +1,126 @@ +/** + * @file event_data.h + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-17 + * + * @copyright Copyright (c) 2025 + * + */ +#ifndef APPS_FC_RADIO_SERVICE_EVENT_DATA_H_ +#define APPS_FC_RADIO_SERVICE_EVENT_DATA_H_ + +#include // NOLINT +#include +#include + +#include "lib/simba/mavlink.h" +#include "ara/core/result.h" + +namespace srp { +namespace apps { +class EventData; +static std::shared_ptr event_data = nullptr; + +class EventData { + private: + __mavlink_simba_tank_temperature_t temp; + __mavlink_simba_tank_pressure_t press; + __mavlink_simba_gps_t gps; + __mavlink_simba_actuator_t actuator; + std::mutex mtx_; + template + void SetValue(T res, T* field) { + std::unique_lock lock(this->mtx_); + *field = res; + } + + public: + static std::shared_ptr GetInstance() { + if (event_data == nullptr) { + event_data = std::make_shared(); + } + return event_data; +} + + void SetTemp1(uint16_t res) { + SetValue(res, &this->temp.temp1); + } + + void SetTemp2(uint16_t res) { + SetValue(res, &this->temp.temp2); + } + + void SetTemp3(uint16_t res) { + SetValue(res, &this->temp.temp3); + } + + void SetDPress(float res) { + SetValue(res, &this->press.Dpressure); + } + + void SetPress(float res) { + SetValue(res, &this->press.pressure); + } + + void SetGPS(int32_t lon, int32_t lat) { + std::unique_lock lock(this->mtx_); + this->gps.lat = lat; + this->gps.lon = lon; + } + + + + void SetActuatorBit(const std::uint8_t res, uint8_t bit_position) { + if (bit_position > 7 || bit_position < 0 || res > 1) { + return; + } + std::unique_lock lock(this->mtx_); + this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res << bit_position); + } + + uint16_t GetTemp1() { + std::unique_lock lock(this->mtx_); + return this->temp.temp1; + } + + uint16_t GetTemp2() { + std::unique_lock lock(this->mtx_); + return this->temp.temp2; + } + + uint16_t GetTemp3() { + std::unique_lock lock(this->mtx_); + return this->temp.temp3; + } + + float GetDPress() { + std::unique_lock lock(this->mtx_); + return this->press.Dpressure; + } + + float GetPress() { + std::unique_lock lock(this->mtx_); + return this->press.pressure; + } + + int32_t GetGPSLat() { + std::unique_lock lock(this->mtx_); + return this->gps.lat; + } + + int32_t GetGPSLon() { + std::unique_lock lock(this->mtx_); + return this->gps.lon; + } + + uint8_t GetActuator() { + std::unique_lock lock(this->mtx_); + return this->actuator.values; + } +}; +} // namespace apps +} // namespace srp + +#endif // APPS_FC_RADIO_SERVICE_EVENT_DATA_H_ diff --git a/apps/fc/radio_service/radio_app.cc b/apps/fc/radio_service/radio_app.cc index d9029aac..7f7711e8 100644 --- a/apps/fc/radio_service/radio_app.cc +++ b/apps/fc/radio_service/radio_app.cc @@ -25,47 +25,47 @@ constexpr auto kServo_service_path_name = "srp/apps/RadioApp/ServoService"; constexpr auto kRecovery_service_path_name = "srp/apps/RadioApp/RecoveryService"; constexpr auto KGPS_UART_path = "/dev/ttyS1"; constexpr auto KGPS_UART_baudrate = B115200; -constexpr auto systemId = 1; +constexpr auto kSystemId = 1; } // namespace void RadioApp::TransmittingLoop(const std::stop_token& token) { - core::timestamp::TimestampController timestamp_; mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - timestamp_.Init(); + timestamp_->Init(); + event_data = EventData::GetInstance(); while (!token.stop_requested()) { // TODO(m.mankowski2004@gmail.com): Change the component IDs - std::unique_lock lock(this->mutex_); - mavlink_msg_simba_tank_temperature_pack(systemId, 200, &msg, - temp.temp1, temp.temp2, temp.temp3); - uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); - uart_->Write(std::vector(buffer, buffer + len)); - - mavlink_msg_simba_tank_pressure_pack(systemId, 200, &msg, - press.Dpressure, press.pressure); - len = mavlink_msg_to_send_buffer(buffer, &msg); - uart_->Write(std::vector(buffer, buffer + len)); - - mavlink_msg_simba_gps_pack(systemId, 200, &msg, - gps.lon, gps.lat, 0); // add altitude later - len = mavlink_msg_to_send_buffer(buffer, &msg); - uart_->Write(std::vector(buffer, buffer + len)); - - auto val = timestamp_.GetNewTimeStamp(); - if (val.has_value()) { - mavlink_msg_simba_heartbeat_pack(systemId, 200, &msg, - static_cast(val.value()), 0, 0); // add later the status of both computers - len = mavlink_msg_to_send_buffer(buffer, &msg); + auto start = std::chrono::high_resolution_clock::now(); + { + auto send = [&](auto pack_func) { + pack_func(); + uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); uart_->Write(std::vector(buffer, buffer + len)); + }; + auto temp1 = event_data->GetTemp1(); + auto temp2 = event_data->GetTemp2(); + auto temp3 = event_data->GetTemp3(); + auto Dpress = event_data->GetDPress(); + auto press = event_data->GetPress(); + auto gpsLat = event_data->GetGPSLat(); + auto gpsLon = event_data->GetGPSLon(); + auto actuator = event_data->GetActuator(); + send([&] { mavlink_msg_simba_tank_temperature_pack(kSystemId, 200, &msg, temp1, temp2, temp3); }); + send([&] { mavlink_msg_simba_tank_pressure_pack(kSystemId, 200, &msg, Dpress, press); }); + // TODO(m.mankowski2004@gmail.com): change altitude + send([&] { mavlink_msg_simba_gps_pack(kSystemId, 200, &msg, gpsLon, gpsLat, 0); }); + auto val = timestamp_->GetNewTimeStamp(); + if (val.has_value()) { + // TODO(m.mankowski2004@gmail.com): add later the status of both computers + send([&] { mavlink_msg_simba_heartbeat_pack(kSystemId, 200, &msg, static_cast(val.value()), 0, 0); }); } - mavlink_msg_simba_actuator_pack(systemId, 200, &msg, - actuator.values); - len = mavlink_msg_to_send_buffer(buffer, &msg); - uart_->Write(std::vector(buffer, buffer + len)); - lock.unlock(); - core::condition::wait_for(std::chrono::milliseconds(1000), token); // 1 Hz + send([&] { mavlink_msg_simba_actuator_pack(kSystemId, 200, &msg, actuator); }); + } + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start); + core::condition::wait_for(std::chrono::milliseconds(1000 - duration.count()), token); // 1 Hz } } @@ -80,22 +80,29 @@ int RadioApp::Run(const std::stop_token& token) { return core::ErrorCode::kOk; } -void RadioApp::Init(std::unique_ptr uart) { +void RadioApp::InitUart(std::unique_ptr uart) { this->uart_ = std::move(uart); } + +void RadioApp::InitTimestamp(std::unique_ptr timestamp) { + this->timestamp_ = std::move(timestamp); +} + int RadioApp::Initialize(const std::map parms) { if (!this->uart_) { auto uart_d = std::make_unique(); - Init(std::move(uart_d)); + InitUart(std::move(uart_d)); } if (!this->uart_->Open(KGPS_UART_path, KGPS_UART_baudrate)) { return 1; } - this->service_ipc = std::make_unique - (this->service_ipc_instance); - this->service_udp = std::make_unique - (this->service_udp_instance); + if (!this->timestamp_) { + auto timestamp_d = std::make_unique(); + InitTimestamp(std::move(timestamp_d)); + } + service_ipc = std::make_unique(service_ipc_instance); + service_udp = std::make_unique(service_udp_instance); service_ipc->StartOffer(); service_udp->StartOffer(); this->SomeIpInit(); @@ -110,8 +117,7 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); - this->temp.temp1 = res.Value(); + event_data->SetTemp1(res.Value()); }); }); env_service_handler->newTempEvent_2.Subscribe(1, [this](const uint8_t status) { @@ -120,8 +126,7 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); - this->temp.temp2 = res.Value(); + event_data->SetTemp2(res.Value()); }); }); env_service_handler->newTempEvent_3.Subscribe(1, [this](const uint8_t status) { @@ -130,8 +135,7 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); - this->temp.temp3 = res.Value(); + event_data->SetTemp3(res.Value()); }); }); env_service_handler->newDPressEvent.Subscribe(1, [this](const uint8_t status) { @@ -140,8 +144,7 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); - this->press.Dpressure = res.Value(); + event_data->SetDPress(res.Value()); }); }); env_service_handler->newPressEvent.Subscribe(1, [this](const uint8_t status) { @@ -150,8 +153,7 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); - this->press.pressure = res.Value(); + event_data->SetPress(res.Value()); }); }); }); @@ -163,9 +165,7 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); - std::memcpy(&this->gps.lon, &res.Value().longitude, sizeof(float)); - std::memcpy(&this->gps.lat, &res.Value().latitude, sizeof(float)); + event_data->SetGPS(res.Value().longitude, res.Value().latitude); }); }); }); @@ -177,9 +177,8 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); uint8_t bit_position = 0; - this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res.Value() << bit_position); + event_data->SetActuatorBit(res.Value(), bit_position); }); }); }); @@ -191,9 +190,8 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); uint8_t bit_position = 1; - this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res.Value() << bit_position); + event_data->SetActuatorBit(res.Value(), bit_position); }); }); servo_service_handler->ServoVentStatusEvent.Subscribe(1, [this](const uint8_t status) { @@ -202,9 +200,8 @@ void RadioApp::SomeIpInit() { if (!res.HasValue()) { return; } - std::unique_lock lock(this->mutex_); uint8_t bit_position = 2; - this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res.Value() << bit_position); + event_data->SetActuatorBit(res.Value(), bit_position); }); }); }); diff --git a/apps/fc/radio_service/radio_app.h b/apps/fc/radio_service/radio_app.h index 4be4074d..edc3215a 100644 --- a/apps/fc/radio_service/radio_app.h +++ b/apps/fc/radio_service/radio_app.h @@ -18,6 +18,7 @@ #include "ara/exec/adaptive_application.h" #include "lib/simba/mavlink.h" +#include "apps/fc/radio_service/event_data.h" #include "srp/env/EnvApp/EnvAppHandler.h" #include "srp/apps/GPSService/GPSServiceHandler.h" #include "srp/apps/PrimerService/PrimerServiceHandler.h" @@ -29,13 +30,9 @@ namespace srp { namespace apps { -class RadioApp final : public ara::exec::AdaptiveApplication { +class RadioApp : public ara::exec::AdaptiveApplication { private: - __mavlink_simba_tank_temperature_t temp; - __mavlink_simba_tank_pressure_t press; - __mavlink_simba_gps_t gps; - __mavlink_simba_actuator_t actuator; - std::mutex mutex_; + std::shared_ptr event_data; PrimerServiceProxy primer_service_proxy; std::shared_ptr primer_service_handler; ServoServiceProxy servo_service_proxy; @@ -49,12 +46,16 @@ class RadioApp final : public ara::exec::AdaptiveApplication { std::unique_ptr service_ipc; std::unique_ptr service_udp; std::unique_ptr uart_; + std::unique_ptr timestamp_; void SomeIpInit(); + + protected: + void InitUart(std::unique_ptr uart); + void InitTimestamp(std::unique_ptr timestamp); void TransmittingLoop(const std::stop_token& token); public: int Run(const std::stop_token& token) override; - void Init(std::unique_ptr uart); int Initialize(const std::map parms) override; ~RadioApp(); diff --git a/apps/fc/radio_service/ut/BUILD b/apps/fc/radio_service/ut/BUILD new file mode 100644 index 00000000..0f222e5e --- /dev/null +++ b/apps/fc/radio_service/ut/BUILD @@ -0,0 +1,13 @@ +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", + ], +) \ No newline at end of file diff --git a/apps/fc/radio_service/ut/radio_app_test.cc b/apps/fc/radio_service/ut/radio_app_test.cc new file mode 100644 index 00000000..5b585412 --- /dev/null +++ b/apps/fc/radio_service/ut/radio_app_test.cc @@ -0,0 +1,151 @@ +/** + * @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/fc/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) { + 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: + 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) { + RadioApp::TransmittingLoop(token); + } +}; + +struct EventDataTestParams { + uint16_t input_temp1; + uint16_t input_temp2; + uint16_t input_temp3; + float input_Dpressure; + 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; + std::vector expected_temp; + std::vector expected_pressure; + std::vector expected_gps; + std::vector expected_heartbeat; + std::vector expected_actuator; +}; + +class EventDataTest : public ::testing::TestWithParam {}; + +// EXPECTED SHOULD LOOK LIKE: {STX, payloadLen, seq, systemId, compId, msgId, valuesInUint8_t, CRC_calculated} +INSTANTIATE_TEST_SUITE_P(EventDataTestParameters, EventDataTest, + ::testing::Values( + EventDataTestParams{ + 0, 0, 0, 0, 0, 0, 0, + std::optional{0}, + 0, 0, 0, + {254, 6, 0, 1, 200, 69, 0, 0, 0, 0, 0, 0, 170, 105}, + {254, 8, 1, 1, 200, 70, 0, 0, 0, 0, 0, 0, 0, 0, 178, 119}, + {254, 12, 2, 1, 200, 72, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 50, 93}, + {254, 10, 3, 1, 200, 73, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 182, 219}, + {254, 1, 4, 1, 200, 68, 0, 142, 237} + }, + + EventDataTestParams { + 65535, 65535, 65535, FLT_MAX, FLT_MAX, INT32_MAX, INT32_MAX, + std::optional{INT64_MAX}, + 1, 1, 1, + {254, 6, 5, 1, 200, 69, 255, 255, 255, 255, 255, 255, 106, 205}, // ?????????? + {254, 8, 6, 1, 200, 70, 255, 255, 255, 255, 255, 255, 255, 255, 221, 91}, + {254, 12, 7, 1, 200, 72, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 169, 240}, + {254, 10, 8, 1, 200, 73, 255, 255, 255, 255, 255, 255, 255, 127, 0, 0, 224, 130}, + {254, 1, 9, 1, 200, 68, 7, 204, 233} + } + + // EventDataTestParams{ + // 0, 0, 0, -FLT_MAX, -FLT_MAX, INT32_MIN, INT32_MIN, + // std::optional{INT64_MIN}, + // 0, 0, 0, + // {254, 6, 10, 1, 200, 69, }, + // {254, 8, 11, 1, 200, 70, }, + // {254, 12, 12, 1, 200, 72, }, + // {254, 10, 13, 1, 200, 73, }, + // {254, 1, 14, 1, 200, 68, } + // } + ) +); + +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(); + + event_data->SetTemp1(params.input_temp1); + event_data->SetTemp2(params.input_temp2); + event_data->SetTemp3(params.input_temp3); + event_data->SetDPress(params.input_Dpressure); + event_data->SetPress(params.input_pressure); + event_data->SetGPS(params.input_gpslon, params.input_gpslat); + event_data->SetActuatorBit(params.input_primer, 0); + event_data->SetActuatorBit(params.input_servo, 1); + event_data->SetActuatorBit(params.input_servovent, 2); + + std::jthread transmitting_thread([&wrapper_](const std::stop_token& token) { + wrapper_.TestTransmittingLoop(token); + }); + std::this_thread::sleep_for(std::chrono::milliseconds(900)); + transmitting_thread.request_stop(); + transmitting_thread.join(); + EXPECT_EQ(sent_data.size(), 5); + EXPECT_EQ(sent_data[0], params.expected_temp); + EXPECT_EQ(sent_data[1], params.expected_pressure); + EXPECT_EQ(sent_data[2], params.expected_gps); + EXPECT_EQ(sent_data[3], params.expected_heartbeat); + EXPECT_EQ(sent_data[4], params.expected_actuator); +} diff --git a/core/timestamp/BUILD b/core/timestamp/BUILD index 1e8df73c..19e61978 100644 --- a/core/timestamp/BUILD +++ b/core/timestamp/BUILD @@ -1,6 +1,9 @@ cc_library( name = "timestamp_controller", - hdrs = ["timestamp_driver.hpp"], + hdrs = [ + "timestamp_driver.hpp", + "Itimestamp_driver.h", + ], srcs = ["timestamp_driver.cpp"], visibility = ["//visibility:public"], deps = [ @@ -10,3 +13,14 @@ cc_library( "//core/common:core", ] ) + +cc_library( + name = "mock_timestamp_controller", + srcs = ["mock_timestamp_driver.h"], + visibility = ["//visibility:public"], + deps = [ + "@com_google_googletest//:gtest_main", + "//core/timestamp:timestamp_controller", + ], + testonly = True, +) diff --git a/core/timestamp/Itimestamp_driver.h b/core/timestamp/Itimestamp_driver.h new file mode 100644 index 00000000..683e7d12 --- /dev/null +++ b/core/timestamp/Itimestamp_driver.h @@ -0,0 +1,40 @@ +/** + * @file Itimestamp_driver.h + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-24 + * + * @copyright Copyright (c) 2025 + * + */ + +#ifndef CORE_TIMESTAMP_ITIMESTAMP_DRIVER_H_ +#define CORE_TIMESTAMP_ITIMESTAMP_DRIVER_H_ + +#include +#include + +namespace srp { +namespace core { +namespace timestamp { + +class ITimestampController { + public: + virtual std::optional GetNewTimeStamp() = 0; + virtual void Init() = 0; + virtual ~ITimestampController() = default; +}; + +class ITimestampMaster { + public: + virtual int64_t GetNewTimeStamp() = 0; + virtual void CorrectStartPoint(const int64_t offset) = 0; + virtual void Init() = 0; + virtual ~ITimestampMaster() = default; +}; + +} // namespace timestamp +} // namespace core +} // namespace srp +#endif // CORE_TIMESTAMP_ITIMESTAMP_DRIVER_H_ diff --git a/core/timestamp/mock_timestamp_driver.h b/core/timestamp/mock_timestamp_driver.h new file mode 100644 index 00000000..d96834fd --- /dev/null +++ b/core/timestamp/mock_timestamp_driver.h @@ -0,0 +1,31 @@ +/** + * @file mock_timestamp_driver.h + * @author Michał Mańkowski (m.mankowski2004@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-24 + * + * @copyright Copyright (c) 2025 + * + */ + +#ifndef CORE_TIMESTAMP_MOCK_TIMESTAMP_DRIVER_H_ +#define CORE_TIMESTAMP_MOCK_TIMESTAMP_DRIVER_H_ + +#include +#include "core/timestamp/Itimestamp_driver.h" + +class MockTimestampController : public srp::core::timestamp::ITimestampController { + public: + MOCK_METHOD(std::optional, GetNewTimeStamp, (), (override)); + MOCK_METHOD(void, Init, (), (override)); +}; + +class MockTimestampMaster : public srp::core::timestamp::ITimestampMaster { + public: + MOCK_METHOD(int64_t, GetNewTimeStamp, (), (override)); + MOCK_METHOD(void, CorrectStartPoint, (const int64_t offset), (override)); + MOCK_METHOD(void, Init, (), (override)); +}; + +#endif // CORE_TIMESTAMP_MOCK_TIMESTAMP_DRIVER_H_ diff --git a/core/timestamp/timestamp_driver.hpp b/core/timestamp/timestamp_driver.hpp index bc74fde8..54aa2c7d 100644 --- a/core/timestamp/timestamp_driver.hpp +++ b/core/timestamp/timestamp_driver.hpp @@ -16,31 +16,32 @@ #include // NOLINT #include "bindings/common/shm/shm_skeleton.h" #include "bindings/common/shm/shm_proxy.h" +#include "core/timestamp/Itimestamp_driver.h" namespace srp { namespace core { namespace timestamp { -class TimestampController { +class TimestampController : public ITimestampController { private: const ara::core::InstanceSpecifier instance_; bindings::com::shm::ShmProxy proxy_; public: - std::optional GetNewTimeStamp(); - void Init(); + std::optional GetNewTimeStamp() override; + void Init() override; TimestampController(); }; -class TimestampMaster { +class TimestampMaster : public ITimestampMaster { private: const ara::core::InstanceSpecifier instance_; bindings::com::shm::ShmSkeleton skeleton_; int64_t start; public: TimestampMaster(); - int64_t GetNewTimeStamp(); - void CorrectStartPoint(const int64_t offset); - void Init(); + int64_t GetNewTimeStamp() override; + void CorrectStartPoint(const int64_t offset) override; + void Init() override; }; } // namespace timestamp diff --git a/deployment/apps/fc/radio_app/app_config.json b/deployment/apps/fc/radio_app/app_config.json index 34a3a527..b78e2c91 100644 --- a/deployment/apps/fc/radio_app/app_config.json +++ b/deployment/apps/fc/radio_app/app_config.json @@ -87,4 +87,4 @@ ] } } -} \ No newline at end of file +} diff --git a/deployment/apps/fc/recovery_service/BUILD b/deployment/apps/fc/recovery_service/BUILD index 17f91c95..eeba787a 100644 --- a/deployment/apps/fc/recovery_service/BUILD +++ b/deployment/apps/fc/recovery_service/BUILD @@ -11,7 +11,7 @@ filegroup( visibility = [ "//apps/fc/recovery_service:__subpackages__", "//deployment/apps/fc/recovery_service:__subpackages__", - "//deployment/apps/fc/radio_app:__subpackages__" + "//deployment/apps/fc/radio_app:__subpackages__", ], ) diff --git a/deployment/apps/prim_service/BUILD b/deployment/apps/prim_service/BUILD index f4298afd..c2ba4edf 100644 --- a/deployment/apps/prim_service/BUILD +++ b/deployment/apps/prim_service/BUILD @@ -10,7 +10,7 @@ filegroup( visibility = [ "//apps/prim_service:__subpackages__", "//deployment/apps/engine_app:__subpackages__", - "//deployment/apps/fc/radio_app:__subpackages__" + "//deployment/apps/fc/radio_app:__subpackages__", ], ) diff --git a/deployment/apps/servo_service/BUILD b/deployment/apps/servo_service/BUILD index 71fd991f..4f670ef5 100644 --- a/deployment/apps/servo_service/BUILD +++ b/deployment/apps/servo_service/BUILD @@ -15,7 +15,7 @@ filegroup( "//deployment/apps/servo_service:__subpackages__", "//deployment/apps/engine_app:__subpackages__", "//mw/i2c_service:__subpackages__", - "//deployment/apps/fc/radio_app:__subpackages__" + "//deployment/apps/fc/radio_app:__subpackages__", ], ) diff --git a/deployment/cpu/fc/BUILD b/deployment/cpu/fc/BUILD index a93582eb..f34349f3 100644 --- a/deployment/cpu/fc/BUILD +++ b/deployment/cpu/fc/BUILD @@ -29,6 +29,7 @@ cpu_def( "//deployment/apps/fc/gps_app:GPSApp", "//deployment/apps/fc/recovery_service:RecoveryService", "//deployment/mw/timestamp_mw:timestamp_service", + "//deployment/apps/fc/radio_app:RadioApp" ], ) From 2ddab3c24073257709db795e4c80ffccba8d7839 Mon Sep 17 00:00:00 2001 From: Mateusz Krajewski Date: Sat, 26 Apr 2025 10:19:44 +0200 Subject: [PATCH 3/5] makr fix event_data issue --- apps/fc/radio_service/BUILD | 3 +- apps/fc/radio_service/event_data.cc | 111 ++++++++++++++ apps/fc/radio_service/event_data.h | 115 +++------------ apps/fc/radio_service/radio_app.cc | 1 + apps/fc/radio_service/radio_app.h | 3 +- apps/fc/radio_service/ut/BUILD | 11 ++ apps/fc/radio_service/ut/event_data_test.cc | 152 ++++++++++++++++++++ apps/fc/radio_service/ut/radio_app_test.cc | 9 +- 8 files changed, 307 insertions(+), 98 deletions(-) create mode 100644 apps/fc/radio_service/event_data.cc create mode 100644 apps/fc/radio_service/ut/event_data_test.cc diff --git a/apps/fc/radio_service/BUILD b/apps/fc/radio_service/BUILD index bee4aa41..1cb88f3d 100644 --- a/apps/fc/radio_service/BUILD +++ b/apps/fc/radio_service/BUILD @@ -9,7 +9,8 @@ cc_library( "//core/uart:uart_driver" ], srcs = [ - "radio_app.cc" + "radio_app.cc", + "event_data.cc", ], hdrs = [ "radio_app.h", diff --git a/apps/fc/radio_service/event_data.cc b/apps/fc/radio_service/event_data.cc new file mode 100644 index 00000000..6c5d5a78 --- /dev/null +++ b/apps/fc/radio_service/event_data.cc @@ -0,0 +1,111 @@ +/** + * @file event_data.cc + * @author Mateusz Krajewski (matikrajek42@gmail.com) + * @brief + * @version 0.1 + * @date 2025-04-26 + * + * @copyright Copyright (c) 2025 + * + */ +#include "apps/fc/radio_service/event_data.h" + +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; +} + +template +void EventData::SetValue(T res, T* field) { + std::unique_lock lock(this->mtx_); + *field = res; +} + +void EventData::SetTemp1(uint16_t res) { + SetValue(res, &this->temp.temp1); +} + +void EventData::SetTemp2(uint16_t res) { + SetValue(res, &this->temp.temp2); +} + +void EventData::SetTemp3(uint16_t res) { + SetValue(res, &this->temp.temp3); +} + +void EventData::SetDPress(float res) { + SetValue(res, &this->press.Dpressure); +} + +void EventData::SetPress(float res) { + SetValue(res, &this->press.pressure); +} + +void EventData::SetGPS(int32_t lon, int32_t lat) { + std::unique_lock lock(this->mtx_); + this->gps.lat = lat; + this->gps.lon = lon; +} + +void EventData::SetActuatorBit(uint8_t res, uint8_t bit_position) { + if (bit_position > 7 || res > 1) { + return; + } + std::unique_lock lock(this->mtx_); + this->actuator.values = + (this->actuator.values & ~(1 << bit_position)) | (res << bit_position); +} + +uint16_t EventData::GetTemp1() { + std::unique_lock lock(this->mtx_); + return this->temp.temp1; +} + +uint16_t EventData::GetTemp2() { + std::unique_lock lock(this->mtx_); + return this->temp.temp2; +} + +uint16_t EventData::GetTemp3() { + std::unique_lock lock(this->mtx_); + return this->temp.temp3; +} + +float EventData::GetDPress() { + std::unique_lock lock(this->mtx_); + return this->press.Dpressure; +} + +float EventData::GetPress() { + std::unique_lock lock(this->mtx_); + return this->press.pressure; +} + +int32_t EventData::GetGPSLat() { + std::unique_lock lock(this->mtx_); + return this->gps.lat; +} + +int32_t EventData::GetGPSLon() { + std::unique_lock lock(this->mtx_); + return this->gps.lon; +} + +uint8_t EventData::GetActuator() { + std::unique_lock lock(this->mtx_); + return this->actuator.values; +} + +// Instancja szablonu musi być jawnie zdefiniowana w pliku .cpp +template void EventData::SetValue(uint16_t, uint16_t*); +template void EventData::SetValue(float, float*); + +} // namespace apps +} // namespace srp diff --git a/apps/fc/radio_service/event_data.h b/apps/fc/radio_service/event_data.h index 02fae227..bc4107f1 100644 --- a/apps/fc/radio_service/event_data.h +++ b/apps/fc/radio_service/event_data.h @@ -1,27 +1,14 @@ -/** - * @file event_data.h - * @author Michał Mańkowski (m.mankowski2004@gmail.com) - * @brief - * @version 0.1 - * @date 2025-04-17 - * - * @copyright Copyright (c) 2025 - * - */ #ifndef APPS_FC_RADIO_SERVICE_EVENT_DATA_H_ #define APPS_FC_RADIO_SERVICE_EVENT_DATA_H_ -#include // NOLINT -#include +#include #include +#include #include "lib/simba/mavlink.h" -#include "ara/core/result.h" namespace srp { namespace apps { -class EventData; -static std::shared_ptr event_data = nullptr; class EventData { private: @@ -30,96 +17,36 @@ class EventData { __mavlink_simba_gps_t gps; __mavlink_simba_actuator_t actuator; std::mutex mtx_; + template - void SetValue(T res, T* field) { - std::unique_lock lock(this->mtx_); - *field = res; - } + void SetValue(T res, T* field); public: - static std::shared_ptr GetInstance() { - if (event_data == nullptr) { - event_data = std::make_shared(); - } - return event_data; -} - - void SetTemp1(uint16_t res) { - SetValue(res, &this->temp.temp1); - } - - void SetTemp2(uint16_t res) { - SetValue(res, &this->temp.temp2); - } - - void SetTemp3(uint16_t res) { - SetValue(res, &this->temp.temp3); - } - - void SetDPress(float res) { - SetValue(res, &this->press.Dpressure); - } - - void SetPress(float res) { - SetValue(res, &this->press.pressure); - } + static std::shared_ptr GetInstance(); - void SetGPS(int32_t lon, int32_t lat) { - std::unique_lock lock(this->mtx_); - this->gps.lat = lat; - this->gps.lon = lon; - } + void SetTemp1(uint16_t res); + void SetTemp2(uint16_t res); + void SetTemp3(uint16_t res); + void SetDPress(float res); + void SetPress(float res); + void SetGPS(int32_t lon, int32_t lat); + void SetActuatorBit(uint8_t res, uint8_t bit_position); - void SetActuatorBit(const std::uint8_t res, uint8_t bit_position) { - if (bit_position > 7 || bit_position < 0 || res > 1) { - return; - } - std::unique_lock lock(this->mtx_); - this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res << bit_position); - } + uint16_t GetTemp1(); + uint16_t GetTemp2(); + uint16_t GetTemp3(); - uint16_t GetTemp1() { - std::unique_lock lock(this->mtx_); - return this->temp.temp1; - } + float GetDPress(); + float GetPress(); - uint16_t GetTemp2() { - std::unique_lock lock(this->mtx_); - return this->temp.temp2; - } + int32_t GetGPSLat(); + int32_t GetGPSLon(); - uint16_t GetTemp3() { - std::unique_lock lock(this->mtx_); - return this->temp.temp3; - } - - float GetDPress() { - std::unique_lock lock(this->mtx_); - return this->press.Dpressure; - } - - float GetPress() { - std::unique_lock lock(this->mtx_); - return this->press.pressure; - } - - int32_t GetGPSLat() { - std::unique_lock lock(this->mtx_); - return this->gps.lat; - } - - int32_t GetGPSLon() { - std::unique_lock lock(this->mtx_); - return this->gps.lon; - } - - uint8_t GetActuator() { - std::unique_lock lock(this->mtx_); - return this->actuator.values; - } + uint8_t GetActuator(); }; + } // namespace apps } // namespace srp diff --git a/apps/fc/radio_service/radio_app.cc b/apps/fc/radio_service/radio_app.cc index 7f7711e8..5332bb97 100644 --- a/apps/fc/radio_service/radio_app.cc +++ b/apps/fc/radio_service/radio_app.cc @@ -35,6 +35,7 @@ void RadioApp::TransmittingLoop(const std::stop_token& token) { uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; timestamp_->Init(); event_data = EventData::GetInstance(); + // TODO(m.mankowski2004@gmail.com): catch some error while (!token.stop_requested()) { // TODO(m.mankowski2004@gmail.com): Change the component IDs auto start = std::chrono::high_resolution_clock::now(); diff --git a/apps/fc/radio_service/radio_app.h b/apps/fc/radio_service/radio_app.h index edc3215a..366db0bd 100644 --- a/apps/fc/radio_service/radio_app.h +++ b/apps/fc/radio_service/radio_app.h @@ -32,7 +32,6 @@ namespace srp { namespace apps { class RadioApp : public ara::exec::AdaptiveApplication { private: - std::shared_ptr event_data; PrimerServiceProxy primer_service_proxy; std::shared_ptr primer_service_handler; ServoServiceProxy servo_service_proxy; @@ -53,7 +52,7 @@ class RadioApp : public ara::exec::AdaptiveApplication { void InitUart(std::unique_ptr uart); void InitTimestamp(std::unique_ptr timestamp); void TransmittingLoop(const std::stop_token& token); - + std::shared_ptr event_data; public: int Run(const std::stop_token& token) override; int Initialize(const std::map diff --git a/apps/fc/radio_service/ut/BUILD b/apps/fc/radio_service/ut/BUILD index 0f222e5e..db0fc3fb 100644 --- a/apps/fc/radio_service/ut/BUILD +++ b/apps/fc/radio_service/ut/BUILD @@ -10,4 +10,15 @@ cc_test( "//core/uart:mock_uart", "//core/timestamp:mock_timestamp_controller", ], +) +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", + ], ) \ No newline at end of file diff --git a/apps/fc/radio_service/ut/event_data_test.cc b/apps/fc/radio_service/ut/event_data_test.cc new file mode 100644 index 00000000..17d64020 --- /dev/null +++ b/apps/fc/radio_service/ut/event_data_test.cc @@ -0,0 +1,152 @@ +/** + * @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/fc/radio_service/event_data.h" +using 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()); + uint16_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, UINT16_MAX), // maksymalna wartość uint16_t + std::make_tuple(3, 32767), // środek zakresu + std::make_tuple(1, 42), // losowa wartość + std::make_tuple(2, 9999) // losowa wartość + ) +); + +// --- 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 == "DPress") { + data->SetDPress(value); + EXPECT_FLOAT_EQ(data->GetDPress(), value); + } else if (field == "Press") { + data->SetPress(value); + EXPECT_FLOAT_EQ(data->GetPress(), value); + } else { + FAIL() << "Invalid pressure field"; + } +} + +INSTANTIATE_TEST_SUITE_P( + PressureTests, + PressureTest, + ::testing::Values( + std::make_tuple("DPress", 0.0f), // min + std::make_tuple("Press", 0.0f), // min + std::make_tuple("DPress", 1e10f), // duża wartość + std::make_tuple("Press", -1e5f), // wartość ujemna + std::make_tuple("DPress", 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); + 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 Bit Test --- +class ActuatorBitTest : public ::testing::TestWithParam> { +protected: + std::shared_ptr data; + void SetUp() override { data = EventData::GetInstance(); } +}; + +TEST_P(ActuatorBitTest, SetAndCheckBit) { + auto [value, bit_position, expected] = GetParam(); + data->SetActuatorBit(value, bit_position); + uint8_t actuator = data->GetActuator(); + EXPECT_EQ(((actuator >> bit_position) & 1), expected); +} + +INSTANTIATE_TEST_SUITE_P( + ActuatorBitTests, + ActuatorBitTest, + ::testing::Values( + std::make_tuple(1, 0, 1), // pierwszy bit + std::make_tuple(1, 7, 1), // ostatni bit + std::make_tuple(0, 0, 0), // wyczyszczony pierwszy bit + std::make_tuple(0, 7, 0), // wyczyszczony ostatni bit + std::make_tuple(1, 3, 1), // środkowy ustawiony + std::make_tuple(0, 4, 0) // środkowy wyczyszczony + ) +); + +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()); +} \ No newline at end of file diff --git a/apps/fc/radio_service/ut/radio_app_test.cc b/apps/fc/radio_service/ut/radio_app_test.cc index 5b585412..c0b532da 100644 --- a/apps/fc/radio_service/ut/radio_app_test.cc +++ b/apps/fc/radio_service/ut/radio_app_test.cc @@ -35,6 +35,9 @@ TEST(RADIOAPPTEST, InitializeTestNoUart) { 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)); } @@ -42,7 +45,10 @@ class TestWrapper : public srp::apps::RadioApp { InitTimestamp(std::move(timestamp)); } void TestTransmittingLoop(const std::stop_token& token) { - RadioApp::TransmittingLoop(token); + this->TransmittingLoop(token); + } + std::shared_ptr GetEventDataPtr() { + return this->event_data; } }; @@ -139,6 +145,7 @@ TEST_P(EventDataTest, ValidateEventData) { 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(); From 4bc827b384de1d248b29f704308aa89409278caf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Ma=C5=84kowski?= Date: Wed, 7 May 2025 18:31:28 +0200 Subject: [PATCH 4/5] Fixes --- apps/fc/radio_service/BUILD | 3 +- apps/fc/radio_service/event_data.cc | 22 ++++---- apps/fc/radio_service/event_data.h | 15 +++++- apps/fc/radio_service/radio_app.cc | 52 ++++++++++--------- apps/fc/radio_service/radio_app.h | 4 +- apps/fc/radio_service/ut/BUILD | 29 +++++------ apps/fc/radio_service/ut/event_data_test.cc | 13 ++--- apps/fc/radio_service/ut/radio_app_test.cc | 53 ++++++++------------ deployment/apps/fc/radio_app/app_config.json | 13 +++-- 9 files changed, 108 insertions(+), 96 deletions(-) diff --git a/apps/fc/radio_service/BUILD b/apps/fc/radio_service/BUILD index 1cb88f3d..976a393e 100644 --- a/apps/fc/radio_service/BUILD +++ b/apps/fc/radio_service/BUILD @@ -6,7 +6,8 @@ cc_library( "//deployment/apps/fc/radio_app:someip_lib", "//deployment/apps/fc/radio_app:ara", "//core/timestamp:timestamp_controller", - "//core/uart:uart_driver" + "//core/uart:uart_driver", + "@srp_platform//ara/log", ], srcs = [ "radio_app.cc", diff --git a/apps/fc/radio_service/event_data.cc b/apps/fc/radio_service/event_data.cc index 6c5d5a78..a3f0095e 100644 --- a/apps/fc/radio_service/event_data.cc +++ b/apps/fc/radio_service/event_data.cc @@ -24,7 +24,7 @@ std::shared_ptr EventData::GetInstance() { template void EventData::SetValue(T res, T* field) { - std::unique_lock lock(this->mtx_); + std::unique_lock lock(this->mtx_); *field = res; } @@ -49,7 +49,7 @@ void EventData::SetPress(float res) { } void EventData::SetGPS(int32_t lon, int32_t lat) { - std::unique_lock lock(this->mtx_); + std::unique_lock lock(this->mtx_); this->gps.lat = lat; this->gps.lon = lon; } @@ -58,48 +58,48 @@ void EventData::SetActuatorBit(uint8_t res, uint8_t bit_position) { if (bit_position > 7 || res > 1) { return; } - std::unique_lock lock(this->mtx_); + std::unique_lock lock(this->mtx_); this->actuator.values = (this->actuator.values & ~(1 << bit_position)) | (res << bit_position); } uint16_t EventData::GetTemp1() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->temp.temp1; } uint16_t EventData::GetTemp2() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->temp.temp2; } uint16_t EventData::GetTemp3() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->temp.temp3; } float EventData::GetDPress() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->press.Dpressure; } float EventData::GetPress() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->press.pressure; } int32_t EventData::GetGPSLat() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->gps.lat; } int32_t EventData::GetGPSLon() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->gps.lon; } uint8_t EventData::GetActuator() { - std::unique_lock lock(this->mtx_); + std::shared_lock lock(mtx_); return this->actuator.values; } diff --git a/apps/fc/radio_service/event_data.h b/apps/fc/radio_service/event_data.h index bc4107f1..21beab91 100644 --- a/apps/fc/radio_service/event_data.h +++ b/apps/fc/radio_service/event_data.h @@ -1,7 +1,18 @@ +/** + * @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_FC_RADIO_SERVICE_EVENT_DATA_H_ #define APPS_FC_RADIO_SERVICE_EVENT_DATA_H_ -#include +#include +#include // NOLINT #include #include @@ -16,7 +27,7 @@ class EventData { __mavlink_simba_tank_pressure_t press; __mavlink_simba_gps_t gps; __mavlink_simba_actuator_t actuator; - std::mutex mtx_; + std::shared_mutex mtx_; template void SetValue(T res, T* field); diff --git a/apps/fc/radio_service/radio_app.cc b/apps/fc/radio_service/radio_app.cc index 5332bb97..9fec2bae 100644 --- a/apps/fc/radio_service/radio_app.cc +++ b/apps/fc/radio_service/radio_app.cc @@ -26,47 +26,49 @@ constexpr auto kRecovery_service_path_name = "srp/apps/RadioApp/RecoveryService" constexpr auto KGPS_UART_path = "/dev/ttyS1"; constexpr auto KGPS_UART_baudrate = B115200; constexpr auto kSystemId = 1; +constexpr auto kComponentId = 200; +constexpr auto kTime = 1000; } // namespace - - void RadioApp::TransmittingLoop(const std::stop_token& token) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; timestamp_->Init(); event_data = EventData::GetInstance(); - // TODO(m.mankowski2004@gmail.com): catch some error + auto send = [&](auto pack_func) { + pack_func(); + uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); + mavl_logger.LogDebug() << std::vector(buffer, buffer + len); + uart_->Write(std::vector(buffer, buffer + len)); + }; while (!token.stop_requested()) { - // TODO(m.mankowski2004@gmail.com): Change the component IDs auto start = std::chrono::high_resolution_clock::now(); { - auto send = [&](auto pack_func) { - pack_func(); - uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); - uart_->Write(std::vector(buffer, buffer + len)); - }; - auto temp1 = event_data->GetTemp1(); - auto temp2 = event_data->GetTemp2(); - auto temp3 = event_data->GetTemp3(); - auto Dpress = event_data->GetDPress(); - auto press = event_data->GetPress(); - auto gpsLat = event_data->GetGPSLat(); - auto gpsLon = event_data->GetGPSLon(); - auto actuator = event_data->GetActuator(); - send([&] { mavlink_msg_simba_tank_temperature_pack(kSystemId, 200, &msg, temp1, temp2, temp3); }); - send([&] { mavlink_msg_simba_tank_pressure_pack(kSystemId, 200, &msg, Dpress, press); }); + const auto temp1 = event_data->GetTemp1(); + const auto temp2 = event_data->GetTemp2(); + const auto temp3 = event_data->GetTemp3(); + const auto Dpress = event_data->GetDPress(); + const auto press = event_data->GetPress(); + const auto gpsLat = event_data->GetGPSLat(); + const auto gpsLon = event_data->GetGPSLon(); + const auto actuator = event_data->GetActuator(); + send([&] { mavlink_msg_simba_tank_temperature_pack(kSystemId, kComponentId, &msg, temp1, temp2, temp3); }); + send([&] { mavlink_msg_simba_tank_pressure_pack(kSystemId, kComponentId, &msg, Dpress, press); }); // TODO(m.mankowski2004@gmail.com): change altitude - send([&] { mavlink_msg_simba_gps_pack(kSystemId, 200, &msg, gpsLon, gpsLat, 0); }); + send([&] { mavlink_msg_simba_gps_pack(kSystemId, kComponentId, &msg, gpsLon, gpsLat, 0); }); auto val = timestamp_->GetNewTimeStamp(); if (val.has_value()) { // TODO(m.mankowski2004@gmail.com): add later the status of both computers - send([&] { mavlink_msg_simba_heartbeat_pack(kSystemId, 200, &msg, static_cast(val.value()), 0, 0); }); + send([&] { + mavlink_msg_simba_heartbeat_pack( + kSystemId, kComponentId, &msg, static_cast(val.value()), 0, 0); + }); } - send([&] { mavlink_msg_simba_actuator_pack(kSystemId, 200, &msg, actuator); }); + send([&] { mavlink_msg_simba_actuator_pack(kSystemId, kComponentId, &msg, actuator); }); } auto end = std::chrono::high_resolution_clock::now(); auto duration = std::chrono::duration_cast(end - start); - core::condition::wait_for(std::chrono::milliseconds(1000 - duration.count()), token); // 1 Hz + core::condition::wait_for(std::chrono::milliseconds(kTime - duration.count()), token); // 1 Hz } } @@ -219,7 +221,9 @@ gps_service_handler{nullptr}, 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} { +servo_service_handler{nullptr}, +mavl_logger{ara::log::LoggingMenager::GetInstance()->CreateLogger("MAVL", "", ara::log::LogLevel::kDebug)} +{ } } // namespace apps diff --git a/apps/fc/radio_service/radio_app.h b/apps/fc/radio_service/radio_app.h index 366db0bd..cea8c66c 100644 --- a/apps/fc/radio_service/radio_app.h +++ b/apps/fc/radio_service/radio_app.h @@ -26,12 +26,13 @@ #include "core/timestamp/timestamp_driver.hpp" #include "srp/apps/RadioServiceSkeleton.h" #include "core/uart/uart_driver.hpp" - +#include "ara/log/logging_menager.h" namespace srp { namespace apps { class RadioApp : public ara::exec::AdaptiveApplication { private: + const ara::log::Logger& mavl_logger; PrimerServiceProxy primer_service_proxy; std::shared_ptr primer_service_handler; ServoServiceProxy servo_service_proxy; @@ -53,6 +54,7 @@ class RadioApp : public ara::exec::AdaptiveApplication { void InitTimestamp(std::unique_ptr timestamp); void TransmittingLoop(const std::stop_token& token); std::shared_ptr event_data; + public: int Run(const std::stop_token& token) override; int Initialize(const std::map diff --git a/apps/fc/radio_service/ut/BUILD b/apps/fc/radio_service/ut/BUILD index db0fc3fb..e81f530b 100644 --- a/apps/fc/radio_service/ut/BUILD +++ b/apps/fc/radio_service/ut/BUILD @@ -1,16 +1,17 @@ -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", - ], -) +# 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", +# ], +# ) + cc_test( name = "event_data_test", srcs = [ @@ -21,4 +22,4 @@ cc_test( "@com_google_googletest//:gtest_main", "//apps/fc/radio_service:radio_app_lib", ], -) \ No newline at end of file +) diff --git a/apps/fc/radio_service/ut/event_data_test.cc b/apps/fc/radio_service/ut/event_data_test.cc index 17d64020..18d290aa 100644 --- a/apps/fc/radio_service/ut/event_data_test.cc +++ b/apps/fc/radio_service/ut/event_data_test.cc @@ -10,10 +10,10 @@ */ #include "gtest/gtest.h" #include "apps/fc/radio_service/event_data.h" -using namespace srp::apps; +namespace srp::apps { // --- Temperature Test --- class TemperatureTest : public ::testing::TestWithParam> { -protected: + protected: std::shared_ptr data; void SetUp() override { data = EventData::GetInstance(); } }; @@ -54,7 +54,7 @@ INSTANTIATE_TEST_SUITE_P( // --- Pressure Test --- class PressureTest : public ::testing::TestWithParam> { -protected: + protected: std::shared_ptr data; void SetUp() override { data = EventData::GetInstance(); } }; @@ -88,7 +88,7 @@ INSTANTIATE_TEST_SUITE_P( // --- GPS Test --- class GPSTest : public ::testing::TestWithParam> { -protected: + protected: std::shared_ptr data; void SetUp() override { data = EventData::GetInstance(); } }; @@ -114,7 +114,7 @@ INSTANTIATE_TEST_SUITE_P( // --- Actuator Bit Test --- class ActuatorBitTest : public ::testing::TestWithParam> { -protected: + protected: std::shared_ptr data; void SetUp() override { data = EventData::GetInstance(); } }; @@ -149,4 +149,5 @@ TEST(EventDataSingletonTest, SameInstanceReturned) { EXPECT_EQ(instance1->GetTemp1(), instance2->GetTemp1()); EXPECT_EQ(instance1->GetTemp2(), instance2->GetTemp2()); EXPECT_EQ(instance1->GetTemp3(), instance2->GetTemp3()); -} \ No newline at end of file +} +} // namespace srp::apps diff --git a/apps/fc/radio_service/ut/radio_app_test.cc b/apps/fc/radio_service/ut/radio_app_test.cc index c0b532da..1dc4791b 100644 --- a/apps/fc/radio_service/ut/radio_app_test.cc +++ b/apps/fc/radio_service/ut/radio_app_test.cc @@ -35,21 +35,21 @@ TEST(RADIOAPPTEST, InitializeTestNoUart) { 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; - } + 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 { @@ -88,26 +88,15 @@ INSTANTIATE_TEST_SUITE_P(EventDataTestParameters, EventDataTest, }, EventDataTestParams { - 65535, 65535, 65535, FLT_MAX, FLT_MAX, INT32_MAX, INT32_MAX, + 0xFFFF, 0xFFFF, 0xFFFF, FLT_MAX, FLT_MAX, 0x7FFFFFFF, 0x7FFFFFFF, std::optional{INT64_MAX}, 1, 1, 1, - {254, 6, 5, 1, 200, 69, 255, 255, 255, 255, 255, 255, 106, 205}, // ?????????? - {254, 8, 6, 1, 200, 70, 255, 255, 255, 255, 255, 255, 255, 255, 221, 91}, - {254, 12, 7, 1, 200, 72, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 169, 240}, - {254, 10, 8, 1, 200, 73, 255, 255, 255, 255, 255, 255, 255, 127, 0, 0, 224, 130}, - {254, 1, 9, 1, 200, 68, 7, 204, 233} + {254, 6, 5, 1, 200, 69, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 90, 24}, + {254, 8, 6, 1, 200, 70, 0xFF, 0xFF, 0x7F, 0x7F, 0xFF, 0xFF, 0x7F, 0x7F, 66, 88}, + {254, 12, 7, 1, 200, 72, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0x7F, 0, 0, 0, 0, 253, 87}, + {254, 10, 8, 1, 200, 73, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0, 0, 224, 130}, + {254, 1, 9, 1, 200, 68, 7, 89, 149} } - - // EventDataTestParams{ - // 0, 0, 0, -FLT_MAX, -FLT_MAX, INT32_MIN, INT32_MIN, - // std::optional{INT64_MIN}, - // 0, 0, 0, - // {254, 6, 10, 1, 200, 69, }, - // {254, 8, 11, 1, 200, 70, }, - // {254, 12, 12, 1, 200, 72, }, - // {254, 10, 13, 1, 200, 73, }, - // {254, 1, 14, 1, 200, 68, } - // } ) ); diff --git a/deployment/apps/fc/radio_app/app_config.json b/deployment/apps/fc/radio_app/app_config.json index b78e2c91..b9b781e1 100644 --- a/deployment/apps/fc/radio_app/app_config.json +++ b/deployment/apps/fc/radio_app/app_config.json @@ -61,8 +61,9 @@ "require": [ { "name": "srp.env.EnvApp as EnvApp", - "on": "ipc", - "instance": 2 + "on": "udp", + "port": "10001", + "instance": 1 }, { "name": "srp.apps.GPSService as GPSService", @@ -71,12 +72,14 @@ }, { "name": "srp.apps.PrimerService as PrimerService", - "on": "ipc", - "instance": 2 + "on": "udp", + "port": "10002", + "instance": 1 }, { "name": "srp.apps.ServoService as ServoService", - "on": "ipc", + "on": "udp", + "port": "10001", "instance": 2 }, { From 2e3c9cef3b23b8d43177e95e716e5695e15269c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Ma=C5=84kowski?= Date: Mon, 12 May 2025 17:07:20 +0200 Subject: [PATCH 5/5] s --- WORKSPACE | 1 + core/timestamp/Itimestamp_driver.h | 5 +++-- core/timestamp/mock_timestamp_driver.h | 4 ++-- core/timestamp/timestamp_driver.hpp | 5 +++-- deployment/system_definition | 2 +- 5 files changed, 10 insertions(+), 7 deletions(-) diff --git a/WORKSPACE b/WORKSPACE index acfee07e..71fc3909 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -21,6 +21,7 @@ install_python() load("@srp_platform//:pip_install.bzl", "pip_install") pip_install() + include_srp_mavlink("0.3") include_gtest_mock() include_json("3.11.3") diff --git a/core/timestamp/Itimestamp_driver.h b/core/timestamp/Itimestamp_driver.h index 683e7d12..debb85d4 100644 --- a/core/timestamp/Itimestamp_driver.h +++ b/core/timestamp/Itimestamp_driver.h @@ -22,7 +22,8 @@ namespace timestamp { class ITimestampController { public: virtual std::optional GetNewTimeStamp() = 0; - virtual void Init() = 0; + virtual int64_t GetDeltaTime(const int64_t now, const int64_t previous) = 0; + virtual bool Init() = 0; virtual ~ITimestampController() = default; }; @@ -30,7 +31,7 @@ class ITimestampMaster { public: virtual int64_t GetNewTimeStamp() = 0; virtual void CorrectStartPoint(const int64_t offset) = 0; - virtual void Init() = 0; + virtual bool Init() = 0; virtual ~ITimestampMaster() = default; }; diff --git a/core/timestamp/mock_timestamp_driver.h b/core/timestamp/mock_timestamp_driver.h index d96834fd..364be5a5 100644 --- a/core/timestamp/mock_timestamp_driver.h +++ b/core/timestamp/mock_timestamp_driver.h @@ -18,14 +18,14 @@ class MockTimestampController : public srp::core::timestamp::ITimestampController { public: MOCK_METHOD(std::optional, GetNewTimeStamp, (), (override)); - MOCK_METHOD(void, Init, (), (override)); + MOCK_METHOD(bool, Init, (), (override)); }; class MockTimestampMaster : public srp::core::timestamp::ITimestampMaster { public: MOCK_METHOD(int64_t, GetNewTimeStamp, (), (override)); MOCK_METHOD(void, CorrectStartPoint, (const int64_t offset), (override)); - MOCK_METHOD(void, Init, (), (override)); + MOCK_METHOD(bool, Init, (), (override)); }; #endif // CORE_TIMESTAMP_MOCK_TIMESTAMP_DRIVER_H_ diff --git a/core/timestamp/timestamp_driver.hpp b/core/timestamp/timestamp_driver.hpp index 54aa2c7d..5b09e473 100644 --- a/core/timestamp/timestamp_driver.hpp +++ b/core/timestamp/timestamp_driver.hpp @@ -27,7 +27,8 @@ class TimestampController : public ITimestampController { bindings::com::shm::ShmProxy proxy_; public: std::optional GetNewTimeStamp() override; - void Init() override; + int64_t GetDeltaTime(const int64_t now, const int64_t previous) override; + bool Init() override; TimestampController(); }; @@ -41,7 +42,7 @@ class TimestampMaster : public ITimestampMaster { TimestampMaster(); int64_t GetNewTimeStamp() override; void CorrectStartPoint(const int64_t offset) override; - void Init() override; + bool Init() override; }; } // namespace timestamp diff --git a/deployment/system_definition b/deployment/system_definition index ef2086de..9a13410d 160000 --- a/deployment/system_definition +++ b/deployment/system_definition @@ -1 +1 @@ -Subproject commit ef2086def1d0e95182e5c788e89a0394c540b75a +Subproject commit 9a13410d3fcfe588291ffef0262df5f9c913e55f