DAER/visualisation-marsh
1
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Track connection to manager and receive MANUAL_CONTROL

Browse source
This commit is contained in:
Marek S. Łukasiewicz 2025-02-18 12:47:35 +01:00
parent 4ddccdaf62
commit c83135a67b
2 changed files with 129 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

111
src/marshconnector.cpp
View file

@ -2,6 +2,7 @@
#include "godot_cpp/classes/engine.hpp"
#include "godot_cpp/classes/font_file.hpp"
#include "godot_cpp/classes/global_constants.hpp"
#include "godot_cpp/classes/packet_peer_udp.hpp"
#include "godot_cpp/core/math.hpp"
#include "godot_cpp/core/memory.hpp"
@ -12,18 +13,29 @@
#include "godot_cpp/variant/transform3d.hpp"
#include "godot_cpp/variant/vector3.hpp"
#include "mavlink/all/mavlink.h" // IWYU pragma: keep; always include the mavlink.h file for selected dialect
#include "mavlink/common/mavlink_msg_command_long.h"
#include "mavlink/common/mavlink_msg_manual_control.h"
#include "mavlink/common/mavlink_msg_sim_state.h"
#include "mavlink/mavlink_helpers.h"
// How much extra bytes are needed on top of message payload length
#define MAVLINK_OVERHEAD 12
#include "mavlink/mavlink_types.h"
using namespace godot;
void MarshConnector::_bind_methods() {
// Data access
ClassDB::bind_method(D_METHOD("get_aircraft"), &MarshConnector::get_aircraft);
ClassDB::bind_method(D_METHOD("get_cyclic"), &MarshConnector::get_cyclic);
ClassDB::bind_method(D_METHOD("get_collective"),
&MarshConnector::get_collective);
ClassDB::bind_method(D_METHOD("get_pedals"), &MarshConnector::get_pedals);
// Timer callbacks
ClassDB::bind_method(D_METHOD("send_heartbeat"),
&MarshConnector::send_heartbeat);
ClassDB::bind_method(D_METHOD("get_aircraft"), &MarshConnector::get_aircraft);
ClassDB::bind_method(D_METHOD("manager_timeout"),
&MarshConnector::manager_timeout);
// ClassDB::bind_method(D_METHOD("model_timeout"),
// &MarshConnector::model_timeout);
}
MarshConnector::MarshConnector() {
@ -38,6 +50,15 @@ MarshConnector::MarshConnector() {
heartbeat_timer->connect("timeout", Callable(this, "send_heartbeat"));
socket = memnew(PacketPeerUDP);
manager_connected = false;
manager_timer = memnew(Timer);
add_child(manager_timer);
manager_timer->set_wait_time(5.0);
manager_timer->set_one_shot(true);
manager_timer->set_autostart(false);
manager_timer->connect("timeout", Callable(this, "manager_timeout"));
}
MarshConnector::~MarshConnector() {
@ -72,17 +93,19 @@ Error MarshConnector::send_heartbeat() {
mavlink_message_t message;
mavlink_msg_heartbeat_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
MAVLINK_COMM_0, &message, &heartbeat);
uint8_t send_buffer[MAVLINK_OVERHEAD + MAVLINK_MSG_ID_HEARTBEAT_LEN];
return send_message(message);
}
Error MarshConnector::send_message(mavlink_message_t message) {
uint8_t send_buffer[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_to_send_buffer(send_buffer, &message);
PackedByteArray array;
for (int i = 0; i < sizeof(send_buffer); i++) {
array.append(send_buffer[i]);
}
// print_line("Data array ", array);
const auto result = socket->put_packet(array);
// print_line("Send result ", result);
return result;
}
@ -109,6 +132,12 @@ void MarshConnector::receive_data(const PackedByteArray &data) {
case MAVLINK_MSG_ID_PARAM_SET:
handle_param(message);
break;
case MAVLINK_MSG_ID_MANUAL_CONTROL:
handle_manual_control(message);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
handle_heartbeat(message);
break;
default:
break;
}
@ -130,6 +159,13 @@ Transform3D MarshConnector::get_aircraft() {
return Transform3D{Basis{last_rotation}, last_location};
}
Vector2 MarshConnector::get_cyclic() {
return Vector2{last_controls.x, last_controls.y};
}
float MarshConnector::get_collective() { return last_controls.w; }
float MarshConnector::get_pedals() { return last_controls.z; }
Vector2 MarshConnector::local_meters_from_global_degrees(double latitude,
double longitude) {
@ -212,3 +248,64 @@ void MarshConnector::handle_attitude(mavlink_message_t message) {
void MarshConnector::handle_param(mavlink_message_t message) {
print_line("Implement handle_param");
}
void MarshConnector::handle_manual_control(mavlink_message_t message) {
if (message.msgid != MAVLINK_MSG_ID_MANUAL_CONTROL)
return;
mavlink_manual_control_t manual_control;
mavlink_msg_manual_control_decode(&message, &manual_control);
const int16_t invalid = 0x7FFF;
if (manual_control.y != invalid)
last_controls.x = manual_control.y / 1000.0f;
if (manual_control.x != invalid)
last_controls.y = manual_control.x / -1000.0f;
if (manual_control.r != invalid)
last_controls.z = manual_control.r / 1000.0f;
if (manual_control.z != invalid)
last_controls.w = manual_control.z / 1000.0f;
}
void MarshConnector::handle_heartbeat(mavlink_message_t message) {
if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT)
return;
// mavlink_heartbeat_t heartbeat;
// mavlink_msg_heartbeat_decode(&message, &heartbeat);
if (message.compid == MARSH_COMP_ID_MANAGER) {
if (!manager_connected) {
print_line("Connected to MARSH Manager");
// subscribe to messages not sent to visualisation node by default
mavlink_command_long_t command;
command.target_system = 1;
command.target_component = MARSH_COMP_ID_MANAGER;
command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
command.confirmation = 0;
command.param1 = static_cast<float>(MAVLINK_MSG_ID_MANUAL_CONTROL);
command.param2 = 0; // Default rate
command.param3 = 0; // Not used
command.param4 = 0;
command.param5 = 0;
command.param6 = 0;
command.param7 = 1; // Address of requestor
mavlink_message_t message_sent;
mavlink_msg_command_long_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
MAVLINK_COMM_0, &message_sent,
&command);
Error result = send_message(message_sent);
if (result != OK) {
print_line("Subscribe send result ", result);
}
}
manager_connected = true;
manager_timer->start();
}
}
void MarshConnector::manager_timeout() {
print_line("Lost connection to MARSH Manager");
manager_connected = false;
}