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

Send HEARTBEAT mesage from Godot

Browse source
This commit is contained in:
Marek S. Łukasiewicz 2025-01-28 17:32:28 +01:00
parent 7890c90380
commit f46887d6cc
3 changed files with 47 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

42
src/marshconnector.cpp
View file

@ -1,8 +1,16 @@
#include "marshconnector.h"
#include "godot_cpp/classes/engine.hpp"
#include "godot_cpp/classes/font_file.hpp"
#include "godot_cpp/classes/packet_peer_udp.hpp"
#include "godot_cpp/core/memory.hpp"
#include "godot_cpp/core/print_string.hpp"
#include "godot_cpp/variant/packed_byte_array.hpp"
#include "mavlink/all/mavlink.h" // IWYU pragma: keep; always include the mavlink.h file for selected dialect
#include "mavlink/mavlink_helpers.h"
// How much extra bytes are needed on top of message payload length
#define MAVLINK_OVERHEAD 12
using namespace godot;
@ -22,6 +30,8 @@ MarshConnector::MarshConnector() {
heartbeat_timer->set_autostart(true);
heartbeat_timer->connect("timeout", Callable(this, "send_heartbeat"));
socket = memnew(PacketPeerUDP);
if (Engine::get_singleton()->is_editor_hint()) {
// Don't run _process() in the editor
set_process_mode(Node::ProcessMode::PROCESS_MODE_DISABLED);
@ -32,8 +42,38 @@ MarshConnector::~MarshConnector() {
// Free only manually managed member variables
}
void MarshConnector::_process(double delta) { time_passed += delta; }
void MarshConnector::_ready() { print_line("MarshConnector ready"); }
void MarshConnector::_process(double delta) {
time_passed += delta;
if (!socket->is_socket_connected()) {
socket->connect_to_host("127.0.0.1", 24400);
}
}
void MarshConnector::send_heartbeat() {
print_line("Sending HEARTBEAT at ", time_passed, " seconds");
mavlink_heartbeat_t heartbeat;
heartbeat.type = MAV_TYPE_HELICOPTER;
heartbeat.autopilot = MAV_AUTOPILOT_INVALID;
heartbeat.base_mode = 0; // none of the flags applicable
heartbeat.custom_mode = 0; // not used so far
heartbeat.system_status = MAV_STATE_ACTIVE;
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];
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);
}