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

Spin main rotor when receiving data from flight model›

Browse source
This commit is contained in:
Marek S. Łukasiewicz 2025-02-18 15:29:06 +01:00
parent ab91016524
commit 2869d390f8
4 changed files with 83 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

63
src/marshconnector.cpp
View file

@ -12,6 +12,7 @@
#include "godot_cpp/variant/quaternion.hpp"
#include "godot_cpp/variant/transform3d.hpp"
#include "godot_cpp/variant/vector3.hpp"
#include "godot_cpp/variant/vector4.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"
@ -28,14 +29,18 @@ void MarshConnector::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_collective"),
&MarshConnector::get_collective);
ClassDB::bind_method(D_METHOD("get_pedals"), &MarshConnector::get_pedals);
ClassDB::bind_method(D_METHOD("get_model_connected"),
&MarshConnector::get_model_connected);
ClassDB::bind_method(D_METHOD("get_manager_connected"),
&MarshConnector::get_manager_connected);
// Timer callbacks
ClassDB::bind_method(D_METHOD("send_heartbeat"),
&MarshConnector::send_heartbeat);
ClassDB::bind_method(D_METHOD("manager_timeout"),
&MarshConnector::manager_timeout);
// ClassDB::bind_method(D_METHOD("model_timeout"),
// &MarshConnector::model_timeout);
ClassDB::bind_method(D_METHOD("model_timeout"),
&MarshConnector::model_timeout);
}
MarshConnector::MarshConnector() {
@ -59,6 +64,15 @@ MarshConnector::MarshConnector() {
manager_timer->set_one_shot(true);
manager_timer->set_autostart(false);
manager_timer->connect("timeout", Callable(this, "manager_timeout"));
model_connected = false;
model_timer = memnew(Timer);
add_child(model_timer);
model_timer->set_wait_time(1.0);
model_timer->set_one_shot(true);
model_timer->set_autostart(false);
model_timer->connect("timeout", Callable(this, "model_timeout"));
}
MarshConnector::~MarshConnector() {
@ -145,6 +159,30 @@ void MarshConnector::receive_data(const PackedByteArray &data) {
}
}
void MarshConnector::receive_model_data() {
if (!model_connected) {
print_line("Started receiving flight model data");
}
model_connected = true;
model_timer->start();
}
void MarshConnector::model_timeout() {
print_line("Stopped receiving flight model data");
model_connected = false;
}
void MarshConnector::receive_location(Vector3 location) {
last_location = location;
receive_model_data();
}
void MarshConnector::receive_rotation(Quaternion rotation) {
last_rotation = rotation;
receive_model_data();
}
Error MarshConnector::set_parameter(const String &id, float value) {
print_line("Implement set_parameter");
return ERR_METHOD_NOT_FOUND;
@ -164,8 +202,13 @@ Vector2 MarshConnector::get_cyclic() {
}
float MarshConnector::get_collective() { return last_controls.w; }
float MarshConnector::get_pedals() { return last_controls.z; }
bool MarshConnector::get_manager_connected() { return manager_connected; }
bool MarshConnector::get_model_connected() { return model_connected; }
Vector2 MarshConnector::local_meters_from_global_degrees(double latitude,
double longitude) {
@ -197,10 +240,10 @@ void MarshConnector::handle_sim_state(mavlink_message_t message) {
const Vector2 local_position = local_meters_from_global_degrees(lat, lon);
last_location = godot_location_from_mavlink(local_position.x,
local_position.y, sim_state.alt);
last_rotation = godot_rotation_from_mavlink(sim_state.roll, sim_state.pitch,
sim_state.yaw);
receive_location(godot_location_from_mavlink(
local_position.x, local_position.y, sim_state.alt));
receive_rotation(godot_rotation_from_mavlink(sim_state.roll, sim_state.pitch,
sim_state.yaw));
}
Vector3 MarshConnector::godot_location_from_mavlink(float north_meters,
float east_meters,
@ -231,8 +274,8 @@ void MarshConnector::handle_local_position(mavlink_message_t message) {
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);
receive_location(
godot_location_from_mavlink(local_pos.x, local_pos.y, -local_pos.z));
}
void MarshConnector::handle_attitude(mavlink_message_t message) {
@ -241,8 +284,8 @@ void MarshConnector::handle_attitude(mavlink_message_t message) {
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&message, &attitude);
last_rotation =
godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw);
receive_rotation(
godot_rotation_from_mavlink(attitude.roll, attitude.pitch, attitude.yaw));
}
void MarshConnector::handle_param(mavlink_message_t message) {