From c83135a67b33b25b2ab550df89ee1e52ec7f3339 Mon Sep 17 00:00:00 2001 From: "Marek S. Lukasiewicz" Date: Tue, 18 Feb 2025 12:47:35 +0100 Subject: [PATCH] Track connection to manager and receive MANUAL_CONTROL --- src/marshconnector.cpp | 111 ++++++++++++++++++++++++++++++++++++++--- src/marshconnector.h | 25 ++++++++++ 2 files changed, 129 insertions(+), 7 deletions(-) diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index 3a1990c..0d171a6 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -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(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; +} diff --git a/src/marshconnector.h b/src/marshconnector.h index fff1971..cb7eb49 100644 --- a/src/marshconnector.h +++ b/src/marshconnector.h @@ -33,8 +33,25 @@ public: Error set_parameter(const String &id, float value); // Get current parameter value float get_parameter(const String &id); + // Get current state of the aircraft Transform3D get_aircraft(); + // Get normalized (-1 to 1) cyclic position, where X is right, Y is pitch up + Vector2 get_cyclic(); + // Get normalized (0 to 1) collective position, positive to climb + float get_collective(); + // Get normalized (-1 to 1) pedals position, positive turn right + float get_pedals(); + + // Is there a connection to MARSH Manager + bool get_manager_connected(); + // Is receiving data from flight model + bool get_model_connected(); + + // Called by timer + void manager_timeout(); + // // Called by timer + // void model_timeout(); // Convert from global coordinates to local position x=north, y=east // taking into account current configuration. @@ -55,6 +72,10 @@ private: void handle_local_position(mavlink_message_t message); void handle_attitude(mavlink_message_t message); void handle_param(mavlink_message_t message); + void handle_manual_control(mavlink_message_t message); + void handle_heartbeat(mavlink_message_t message); + + Error send_message(mavlink_message_t message); // Do not specify specific values, to ensure PARAM_COUNT is correct // Not an enum class on purpose, to make use more convenient, it's scoped @@ -78,10 +99,14 @@ private: // TODO: Interpolate with some delay Vector3 last_location; Quaternion last_rotation; + Vector4 last_controls; double time_passed; Timer *heartbeat_timer; PacketPeerUDP *socket; + + bool manager_connected; + Timer *manager_timer; }; } // namespace godot