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

8
project/main-xr.tscn
View file

@ -22,7 +22,6 @@ sky = SubResource("Sky_0xm2m")
transform = Transform3D(-1, 0, -8.74228e-08, 0, 1, 0, 8.74228e-08, 0, -1, 0, 0, -3) transform = Transform3D(-1, 0, -8.74228e-08, 0, 1, 0, 8.74228e-08, 0, -1, 0, 0, -3)
[node name="MarshConnector" type="MarshConnector" parent="." index="1"] [node name="MarshConnector" type="MarshConnector" parent="." index="1"]
process_mode = 0
[node name="Aircraft" type="Node3D" parent="." index="2" node_paths=PackedStringArray("connector")] [node name="Aircraft" type="Node3D" parent="." index="2" node_paths=PackedStringArray("connector")]
script = ExtResource("1_hips3") script = ExtResource("1_hips3")
@ -31,12 +30,9 @@ connector = NodePath("../MarshConnector")
[node name="MeshInstance3D" type="MeshInstance3D" parent="Aircraft" index="0"] [node name="MeshInstance3D" type="MeshInstance3D" parent="Aircraft" index="0"]
mesh = SubResource("BoxMesh_2w36v") mesh = SubResource("BoxMesh_2w36v")
[node name="Camera3D" type="Camera3D" parent="." index="3"] [node name="DirectionalLight3D" type="DirectionalLight3D" parent="." index="3"]
transform = Transform3D(-1, -2.26267e-08, 8.44439e-08, 0, 0.965926, 0.258819, -8.74228e-08, 0.258819, -0.965926, 0, 1, -3)
[node name="DirectionalLight3D" type="DirectionalLight3D" parent="." index="4"]
transform = Transform3D(-0.707107, -0.683013, 0.183013, 0, 0.258819, 0.965926, -0.707107, 0.683013, -0.183013, 3, 3, 3) transform = Transform3D(-0.707107, -0.683013, 0.183013, 0, 0.258819, 0.965926, -0.707107, 0.683013, -0.183013, 3, 3, 3)
[node name="WorldEnvironment" type="WorldEnvironment" parent="." index="5"] [node name="WorldEnvironment" type="WorldEnvironment" parent="." index="4"]
environment = SubResource("Environment_jdgxj") environment = SubResource("Environment_jdgxj")
camera_attributes = SubResource("CameraAttributesPractical_fpvso") camera_attributes = SubResource("CameraAttributesPractical_fpvso")

1
project/main.tscn
View file

@ -18,7 +18,6 @@ sky = SubResource("Sky_0xm2m")
[node name="Main" type="Node3D"] [node name="Main" type="Node3D"]
[node name="MarshConnector" type="MarshConnector" parent="."] [node name="MarshConnector" type="MarshConnector" parent="."]
process_mode = 0
[node name="Aircraft" type="Node3D" parent="." node_paths=PackedStringArray("connector")] [node name="Aircraft" type="Node3D" parent="." node_paths=PackedStringArray("connector")]
script = ExtResource("1_ig7tw") script = ExtResource("1_ig7tw")

27
src/marshconnector.cpp
View file

@ -38,11 +38,6 @@ MarshConnector::MarshConnector() {
heartbeat_timer->connect("timeout", Callable(this, "send_heartbeat")); heartbeat_timer->connect("timeout", Callable(this, "send_heartbeat"));
socket = memnew(PacketPeerUDP); 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() { MarshConnector::~MarshConnector() {
@ -65,7 +60,7 @@ void MarshConnector::_process(double delta) {
} }
Error MarshConnector::send_heartbeat() { Error MarshConnector::send_heartbeat() {
print_line("Sending HEARTBEAT at ", time_passed, " seconds"); // print_line("Sending HEARTBEAT at ", time_passed, " seconds");
mavlink_heartbeat_t heartbeat; mavlink_heartbeat_t heartbeat;
heartbeat.type = MAV_TYPE_HELICOPTER; heartbeat.type = MAV_TYPE_HELICOPTER;
@ -84,10 +79,10 @@ Error MarshConnector::send_heartbeat() {
for (int i = 0; i < sizeof(send_buffer); i++) { for (int i = 0; i < sizeof(send_buffer); i++) {
array.append(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); const auto result = socket->put_packet(array);
print_line("Send result ", result); // print_line("Send result ", result);
return 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) { 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) { 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) { void MarshConnector::handle_param(mavlink_message_t message) {