From a04f01b7535b45626dda019e540f2409f6bfec2a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Ma=C5=84kowski?= Date: Fri, 6 Jun 2025 15:25:22 +0200 Subject: [PATCH] Mavlink methods mostly done --- WORKSPACE | 2 +- apps/fc/main_service/BUILD | 3 +- apps/fc/radio_service/BUILD | 1 + apps/fc/radio_service/radio_app.cc | 101 +++++++++++++++++-- apps/fc/radio_service/radio_app.h | 7 ++ deployment/apps/fc/main_app/BUILD | 1 + deployment/apps/fc/radio_app/BUILD | 3 +- deployment/apps/fc/radio_app/app_config.json | 8 +- 8 files changed, 116 insertions(+), 10 deletions(-) diff --git a/WORKSPACE b/WORKSPACE index a416e883..f3341a81 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -23,7 +23,7 @@ pip_install() -include_srp_mavlink("0.3") +include_srp_mavlink("0.4") include_gtest_mock() include_json("3.11.3") diff --git a/apps/fc/main_service/BUILD b/apps/fc/main_service/BUILD index f143771a..879e9483 100644 --- a/apps/fc/main_service/BUILD +++ b/apps/fc/main_service/BUILD @@ -10,7 +10,8 @@ cc_binary( "rocketController.hpp", ], visibility = [ - "//deployment/apps/fc/main_app:__subpackages__" + "//deployment/apps/fc/main_app:__subpackages__", + "//apps/fc/radio_service:__subpackages__", ], deps = [ "@srp_platform//ara/exec:adaptive_application_lib", diff --git a/apps/fc/radio_service/BUILD b/apps/fc/radio_service/BUILD index 976a393e..606a5daf 100644 --- a/apps/fc/radio_service/BUILD +++ b/apps/fc/radio_service/BUILD @@ -8,6 +8,7 @@ cc_library( "//core/timestamp:timestamp_controller", "//core/uart:uart_driver", "@srp_platform//ara/log", + "//apps/fc/main_service:MainService", ], srcs = [ "radio_app.cc", diff --git a/apps/fc/radio_service/radio_app.cc b/apps/fc/radio_service/radio_app.cc index 9fec2bae..2335c069 100644 --- a/apps/fc/radio_service/radio_app.cc +++ b/apps/fc/radio_service/radio_app.cc @@ -12,6 +12,7 @@ #include #include "apps/fc/radio_service/radio_app.h" #include "core/common/condition.h" +#include "apps/fc/main_service/rocket_state.h" namespace srp { namespace apps { @@ -28,6 +29,7 @@ constexpr auto KGPS_UART_baudrate = B115200; constexpr auto kSystemId = 1; constexpr auto kComponentId = 200; constexpr auto kTime = 1000; +constexpr auto kBufferSize = 12; } // namespace void RadioApp::TransmittingLoop(const std::stop_token& token) { @@ -72,15 +74,101 @@ void RadioApp::TransmittingLoop(const std::stop_token& token) { } } +void RadioApp::ListeningLoop(const std::stop_token& token) { + mavlink_message_t msg; + mavlink_status_t status; + + while (!token.stop_requested()) { + auto bytes_read_opt = uart_->Read(kBufferSize); + if (!bytes_read_opt.has_value()) { + continue; + } + auto bytes_read = bytes_read_opt.value(); + for (uint8_t byte : bytes_read) { + if (mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status)) { + uint8_t status = 0; + switch (msg.msgid) { + case 144: { + uint8_t abort = RocketState_t::ABORD; + auto result = this->main_service_handler->setMode(abort); + status = result.HasValue() ? result.Value() : false; + break; + } + case 145: { + // uint8_t hold = ; + // auto result = this->main_service_handler->setMode(hold); + // status = result.HasValue() ? result.Value() : false; + break; + } + case 146: { + uint8_t cmd_change = mavlink_msg_simba_cmd_change_get_cmd_change(&msg); + auto result = this->main_service_handler->setMode(cmd_change); + status = result.HasValue() ? result.Value() : false; + } + case 147: { + uint8_t actuator_id = mavlink_msg_simba_actuator_cmd_get_actuator_id(&msg); + uint8_t value = mavlink_msg_simba_actuator_cmd_get_value(&msg); + status = ActuatorCMD(actuator_id, value); + break; + } + } + SendAck(msg.msgid, msg.seq, status); + } + } + } +} + +void RadioApp::SendAck(uint8_t msgId, uint8_t msgSeq, uint8_t status) { + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + // change to actual ack pack func + mavlink_msg_simba_gps_pack(kSystemId, kComponentId, &msg, msgId, msgSeq, status); + 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)); +} + +bool RadioApp::ActuatorCMD(uint8_t actuator_id, uint8_t value) { + switch (actuator_id) { + case 1: { + auto result = this->servo_service_handler->SetMainServoValue(value); + return result.HasValue() ? result.Value() : false; + break; + } + case 2: { + auto result = this->servo_service_handler->SetVentServoValue(value); + return result.HasValue() ? result.Value() : false; + break; + } + case 3: { + if (value == 1) { + auto result = this->recovery_service_handler->OpenReefedParachute(); + return result.HasValue() ? result.Value() : false; + } + break; + } + case 4: { + if (value == 1) { + auto result = this->recovery_service_handler->UnreefeParachute(); + return result.HasValue() ? result.Value() : false; + } + break; + } + } + return false; +} 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; + std::jthread listening_thread([this](const std::stop_token& t) { + this->ListeningLoop(t); + }); + core::condition::wait(token); + service_ipc->StopOffer(); + service_udp->StopOffer(); + uart_->Close(); + return core::ErrorCode::kOk; } void RadioApp::InitUart(std::unique_ptr uart) { @@ -222,9 +310,10 @@ 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}, +main_service_handler{nullptr}, +recovery_service_handler{nullptr}, mavl_logger{ara::log::LoggingMenager::GetInstance()->CreateLogger("MAVL", "", ara::log::LogLevel::kDebug)} { } - } // namespace apps } // namespace srp diff --git a/apps/fc/radio_service/radio_app.h b/apps/fc/radio_service/radio_app.h index cea8c66c..43f3d28a 100644 --- a/apps/fc/radio_service/radio_app.h +++ b/apps/fc/radio_service/radio_app.h @@ -23,6 +23,8 @@ #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/RecoveryService/RecoveryServiceHandler.h" #include "core/timestamp/timestamp_driver.hpp" #include "srp/apps/RadioServiceSkeleton.h" #include "core/uart/uart_driver.hpp" @@ -41,6 +43,8 @@ class RadioApp : public ara::exec::AdaptiveApplication { std::shared_ptr env_service_handler; GPSServiceProxy gps_service_proxy; std::shared_ptr gps_service_handler; + std::shared_ptr main_service_handler; + std::shared_ptr recovery_service_handler; const ara::core::InstanceSpecifier service_ipc_instance; const ara::core::InstanceSpecifier service_udp_instance; std::unique_ptr service_ipc; @@ -53,6 +57,9 @@ 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); + void ListeningLoop(const std::stop_token& token); + bool ActuatorCMD(uint8_t actuator_id, uint8_t value); + void SendAck(uint8_t msgId, uint8_t msgSeq, uint8_t status); std::shared_ptr event_data; public: diff --git a/deployment/apps/fc/main_app/BUILD b/deployment/apps/fc/main_app/BUILD index e9e00247..80d2c6dc 100644 --- a/deployment/apps/fc/main_app/BUILD +++ b/deployment/apps/fc/main_app/BUILD @@ -10,6 +10,7 @@ filegroup( visibility = [ "//apps/fc/main_service:__subpackages__", "//deployment/apps/fc/main_service:__subpackages__", + "//deployment/apps/fc/radio_app:__subpackages__" ], ) diff --git a/deployment/apps/fc/radio_app/BUILD b/deployment/apps/fc/radio_app/BUILD index 582c882c..51745e31 100644 --- a/deployment/apps/fc/radio_app/BUILD +++ b/deployment/apps/fc/radio_app/BUILD @@ -10,7 +10,8 @@ filegroup( "//deployment/apps/fc/gps_app:instance", "//deployment/apps/prim_service:instance", "//deployment/apps/servo_service:instance", - "//deployment/apps/fc/recovery_service:instance" + "//deployment/apps/fc/recovery_service:instance", + "//deployment/apps/fc/main_app:instance" ], visibility = [ "//apps/fc/radio_service:__subpackages__", diff --git a/deployment/apps/fc/radio_app/app_config.json b/deployment/apps/fc/radio_app/app_config.json index b9b781e1..0b66014c 100644 --- a/deployment/apps/fc/radio_app/app_config.json +++ b/deployment/apps/fc/radio_app/app_config.json @@ -5,7 +5,8 @@ "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" + "deployment/system_definition/someip/fc/recovery_service/service.json", + "deployment/system_definition/someip/fc/main_service/service.json" ], "package": "srp.apps", "adaptive_application": { @@ -86,6 +87,11 @@ "name": "srp.apps.RecoveryService as RecoveryService", "on": "ipc", "instance": 2 + }, + { + "name": "srp.apps.MainService as MainService", + "on": "ipc", + "instance": 2 } ] }