diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index abb695e..7dd2846 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -3,10 +3,14 @@ #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/math.hpp" #include "godot_cpp/core/memory.hpp" #include "godot_cpp/core/print_string.hpp" #include "godot_cpp/variant/packed_byte_array.hpp" +#include "godot_cpp/variant/quaternion.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_sim_state.h" #include "mavlink/mavlink_helpers.h" // How much extra bytes are needed on top of message payload length @@ -52,7 +56,7 @@ void MarshConnector::_process(double delta) { } } -void MarshConnector::send_heartbeat() { +Error MarshConnector::send_heartbeat() { print_line("Sending HEARTBEAT at ", time_passed, " seconds"); mavlink_heartbeat_t heartbeat; @@ -76,4 +80,116 @@ void MarshConnector::send_heartbeat() { const auto result = socket->put_packet(array); print_line("Send result ", result); + return result; +} + +void MarshConnector::receive_data(const PackedByteArray &data) { + mavlink_message_t message; + mavlink_status_t parser_status; + + for (size_t i = 0; i < data.size(); i++) { + + if (mavlink_parse_char(MAVLINK_COMM_0, data.ptr()[i], &message, + &parser_status)) { + switch (message.msgid) { + case MAVLINK_MSG_ID_SIM_STATE: + handle_sim_state(message); + break; + case MAVLINK_MSG_ID_LOCAL_POSITION_NED: + handle_local_position(message); + break; + case MAVLINK_MSG_ID_ATTITUDE: + handle_attitude(message); + break; + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + case MAVLINK_MSG_ID_PARAM_SET: + handle_param(message); + break; + default: + break; + } + } + } +} + +Error MarshConnector::set_parameter(const String &id, float value) { + print_line("Implement set_parameter"); + return ERR_METHOD_NOT_FOUND; +} + +float MarshConnector::get_parameter(const String &id) { + print_line("Implement get_parameter"); + return Math_NAN; +} + +Vector2 MarshConnector::local_meters_from_global_degrees(double latitude, + double longitude) { + + // Convert everything to radians + const double lat = latitude * Math_PI / 180.0; + const double lon = longitude * Math_PI / 180.0; + const double lat0 = parameters[LOCAL_FRAME_LAT] * M_PI / 180.0; + const double lon0 = parameters[LOCAL_FRAME_LON] * M_PI / 180.0; + + double EARTH_RADIUS = 6371008.7714; // mean radius for WGS-84, in meters + + // X North is just length along the meridian + double x = (lat - lat0) * EARTH_RADIUS; + // Y East is just length along the circle of latitude + double y = (lon - lon0) * (EARTH_RADIUS * cos(lat0)); + + return Vector2(x, y); +} +void MarshConnector::handle_sim_state(mavlink_message_t message) { + if (message.msgid != MAVLINK_MSG_ID_SIM_STATE) + return; + + mavlink_sim_state_t sim_state; + mavlink_msg_sim_state_decode(&message, &sim_state); + const double lat = + sim_state.lat_int != 0 ? sim_state.lat_int / 1.0e7 : sim_state.lat; + const double lon = + sim_state.lon_int != 0 ? sim_state.lon_int / 1.0e7 : sim_state.lon; + + 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); +} +Vector3 MarshConnector::godot_location_from_mavlink(float north_meters, + float east_meters, + float alt_meters) { + // See Godot documentation for axis conventions: + // https://docs.godotengine.org/en/stable/classes/class_vector3.html#constants + return Vector3{ + east_meters, // East is RIGHT = Vector3(1, 0, 0) + alt_meters, // Up is UP = Vector3(0, 1, 0) + north_meters, // North is FORWARD = Vector3(0, 0, -1) + }; +} +Quaternion MarshConnector::godot_rotation_from_mavlink(float roll_rad, + float pitch_rad, + float yaw_rad) { + // Checked manually in Godot editor + const Vector3 euler = Vector3{ + -pitch_rad, + -yaw_rad, + roll_rad, + }; + return Quaternion::from_euler(euler); +} + +void MarshConnector::handle_local_position(mavlink_message_t message) { + print_line("Implement handle_local_position"); +} + +void MarshConnector::handle_attitude(mavlink_message_t message) { + print_line("Implement handle_attitude"); +} + +void MarshConnector::handle_param(mavlink_message_t message) { + print_line("Implement handle_param"); } diff --git a/src/marshconnector.h b/src/marshconnector.h index e1397e5..d3dfff5 100644 --- a/src/marshconnector.h +++ b/src/marshconnector.h @@ -4,17 +4,16 @@ #include "godot_cpp/classes/node.hpp" #include "godot_cpp/classes/packet_peer_udp.hpp" #include "godot_cpp/classes/timer.hpp" +#include "godot_cpp/variant/quaternion.hpp" +#include "godot_cpp/variant/vector2.hpp" +#include "godot_cpp/variant/vector3.hpp" +#include "mavlink/mavlink_types.h" namespace godot { class MarshConnector : public Node { GDCLASS(MarshConnector, Node) -private: - double time_passed; - Timer *heartbeat_timer; - PacketPeerUDP *socket; - protected: static void _bind_methods(); @@ -25,7 +24,61 @@ public: void _ready() override; void _process(double delta) override; - void send_heartbeat(); + // Returns the error from put_packet + Error send_heartbeat(); + // Handle MAVLink messages fully contained in data + void receive_data(const PackedByteArray &data); + // Set parameter value with some validation + Error set_parameter(const String &id, float value); + // Get current parameter value + float get_parameter(const String &id); + + // Convert from global coordinates to local position x=north, y=east + // taking into account current configuration. + // Note that 32-bit float type is not exact enough to store global position + Vector2 local_meters_from_global_degrees(double latitude, double longitude); + + static Vector3 godot_location_from_mavlink(float north_meters, + float east_meters, + float alt_meters); + static Quaternion godot_rotation_from_mavlink(float roll_rad, float pitch_rad, + float yaw_rad); + +private: + // Handle corresponding MAVLink message received. + // The functions ensure the type is correct; message_t is used, only + // to keep the amount of code required in the header small. + void handle_sim_state(mavlink_message_t message); + void handle_local_position(mavlink_message_t message); + void handle_attitude(mavlink_message_t message); + void handle_param(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 + // inside the class namespace anyway + enum Parameter { + // FOG_DENSITY, + NAV_OFS_HDG, + NAV_OFS_X, + NAV_OFS_Y, + LOCAL_FRAME_LAT, + LOCAL_FRAME_LON, + PARAM_COUNT, + }; + float parameters[PARAM_COUNT]; + const String parameter_names[PARAM_COUNT] = { + // "FOG_DENSITY", + "NAV_OFS_HDG", "NAV_OFS_X", "NAV_OFS_Y", + "LOCAL_FRAME_LAT", "LOCAL_FRAME_LON", + }; + + // TODO: Interpolate with some delay + Vector3 last_location; + Quaternion last_rotation; + + double time_passed; + Timer *heartbeat_timer; + PacketPeerUDP *socket; }; } // namespace godot