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

Cleanup MarshConnector

Browse source 
Receive ATTITUDE and LOCAL_POSITION_NED
Remove unnecessary prints
This commit is contained in:
Marek S. Łukasiewicz 2025-02-12 15:57:45 +01:00
parent 046388a3eb
commit b868c2bdbb
3 changed files with 19 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

27
src/marshconnector.cpp
View file

@ -38,11 +38,6 @@ MarshConnector::MarshConnector() {
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);
}
}
MarshConnector::~MarshConnector() {
@ -65,7 +60,7 @@ void MarshConnector::_process(double delta) {
}
Error MarshConnector::send_heartbeat() {
print_line("Sending HEARTBEAT at ", time_passed, " seconds");
// print_line("Sending HEARTBEAT at ", time_passed, " seconds");
mavlink_heartbeat_t heartbeat;
heartbeat.type = MAV_TYPE_HELICOPTER;
@ -84,10 +79,10 @@ Error MarshConnector::send_heartbeat() {
for (int i = 0; i < sizeof(send_buffer); i++) {
array.append(send_buffer[i]);
}
print_line("Data array ", array);
// print_line("Data array ", array);
const auto result = socket->put_packet(array);
print_line("Send result ", result);
// print_line("Send result ", result);
return result;
}
@ -195,11 +190,23 @@ Quaternion MarshConnector::godot_rotation_from_mavlink(float roll_rad,
}
void MarshConnector::handle_local_position(mavlink_message_t message) {
print_line("Implement handle_local_position");
if (message.msgid != MAVLINK_MSG_ID_LOCAL_POSITION_NED)
return;
mavlink_local_position_ned_t local_pos;
mavlink_msg_local_position_ned_decode(&message, &local_pos);
last_location =
godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z);
}
void MarshConnector::handle_attitude(mavlink_message_t message) {
print_line("Implement handle_attitude");
if (message.msgid != MAVLINK_MSG_ID_ATTITUDE)
return;
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&message, &attitude);
last_rotation =
godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw);
}
void MarshConnector::handle_param(mavlink_message_t message) {