Skip to content

Added handling methods through mavlink #244

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

Added handling methods through mavlink #244

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion WORKSPACE
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ pip_install()



include_srp_mavlink("0.3")
include_srp_mavlink("0.4")
include_gtest_mock()
include_json("3.11.3")

Expand Down
3 changes: 2 additions & 1 deletion apps/fc/main_service/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
1 change: 1 addition & 0 deletions apps/fc/radio_service/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down
101 changes: 95 additions & 6 deletions apps/fc/radio_service/radio_app.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <vector>
#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 {
Expand All @@ -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;
Copy link
Collaborator

@Mateusz-Krajewski Mateusz-Krajewski Jun 8, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

czemu akurat 12?
skoro masz dostępne MAVLINK_MAX_PACKET_LEN

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Spojrzałem ile bitów trzeba wysłać przed i po wiadomości no i jakiej długości jest największa wiadomość. Wyszło mi 12 z tych szybkich obliczeń.

} // namespace

void RadioApp::TransmittingLoop(const std::stop_token& token) {
Expand Down Expand Up @@ -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)) {
Comment on lines +82 to +88
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

skoro i tak to potem dzielisz na uint8, to moze odczytywać po 1 bajcie z uart?

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) {
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

zmien argumenty na enumy z simba mavlink, jak bedziesz mial nowa wersje

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<uint8_t>(buffer, buffer + len);
uart_->Write(std::vector<uint8_t>(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<core::uart::IUartDriver> uart) {
Expand Down Expand Up @@ -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
7 changes: 7 additions & 0 deletions apps/fc/radio_service/radio_app.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -41,6 +43,8 @@ class RadioApp : public ara::exec::AdaptiveApplication {
std::shared_ptr<env::EnvAppHandler> env_service_handler;
GPSServiceProxy gps_service_proxy;
std::shared_ptr<GPSServiceHandler> gps_service_handler;
std::shared_ptr<MainServiceHandler> main_service_handler;
std::shared_ptr<RecoveryServiceHandler> recovery_service_handler;
const ara::core::InstanceSpecifier service_ipc_instance;
const ara::core::InstanceSpecifier service_udp_instance;
std::unique_ptr<apps::RadioServiceSkeleton> service_ipc;
Expand All @@ -53,6 +57,9 @@ class RadioApp : public ara::exec::AdaptiveApplication {
void InitUart(std::unique_ptr<core::uart::IUartDriver> uart);
void InitTimestamp(std::unique_ptr<core::timestamp::ITimestampController> 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<EventData> event_data;

public:
Expand Down
1 change: 1 addition & 0 deletions deployment/apps/fc/main_app/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ filegroup(
visibility = [
"//apps/fc/main_service:__subpackages__",
"//deployment/apps/fc/main_service:__subpackages__",
"//deployment/apps/fc/radio_app:__subpackages__"
],
)

Expand Down
3 changes: 2 additions & 1 deletion deployment/apps/fc/radio_app/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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__",
Expand Down
8 changes: 7 additions & 1 deletion deployment/apps/fc/radio_app/app_config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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": {
Expand Down Expand Up @@ -86,6 +87,11 @@
"name": "srp.apps.RecoveryService as RecoveryService",
"on": "ipc",
"instance": 2
},
{
"name": "srp.apps.MainService as MainService",
"on": "ipc",
"instance": 2
}
]
}
Expand Down