diff --git a/README.md b/README.md index 2c4ca5a..e7525a6 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,41 @@ The project is configured to display cockpit instruments with [lidia](https://py Named in this order so not everything starts with the same word. +## Usage + +The application will attempt to start in VR, but if it fails it will display the default pilot viewpoint looking forward. + +### Parameters + +These can be modified over MAVLink, for example using MARSH Manager. + +Example changes to `NAV_OFS_` parameters to start in given positions: + +- Sidestep, vertical repositioning: all default +- Accelerate/decelerate: `HDG` = -90 +- Hover maneuver: `X` = 50, `Y` = 50 +- Start of slalom: `HDG` = -90, `X` = -100, `Y` = 300 + +#### NAV_OFS_HDG + +Displayed heading relative to default position, in degrees. + +#### NAV_OFS_X + +Starting position offset, forward in default orientation, in meters. + +#### NAV_OFS_X + +Starting position offset, right in default orientation, in meters. + +#### LOCAL_FRAME_LAT + +Latitude of local coordinate frame origin when using messages with global position, in degrees. + +#### LOCAL_FRAME_LON + +Longitude of local coordinate frame origin when using messages with global position, in degrees. + ## Development The asset files like textures and models are very big, so the project also uses [Git Large File Storage](https://git-lfs.com/). @@ -77,13 +112,3 @@ scons platform=windows use_mingw=yes use_llvm=yes - Reduce pixel artifacts - Adjust antialiasing - Ensure there are mipmaps and they're used -- Increase sense of urgency/stress when tracking the visual height cue - - Change the perspective to make the ball leave the strip for smaller error - - Time to complete the task - - Configurable length - - Some way to verify completion - - Some way to reset it - - Sound cue for desired and required timing - - Visual timing like progress bar - - Also change color - - On the vertical cue or instrument panel? diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index c53abef..3f66679 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -18,9 +18,14 @@ #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" +#include "mavlink/common/mavlink_msg_param_request_list.h" +#include "mavlink/common/mavlink_msg_param_set.h" +#include "mavlink/common/mavlink_msg_param_value.h" #include "mavlink/common/mavlink_msg_sim_state.h" #include "mavlink/mavlink_helpers.h" #include "mavlink/mavlink_types.h" +#include +#include using namespace godot; @@ -222,8 +227,10 @@ float MarshConnector::get_parameter(const String &id) { Transform3D MarshConnector::get_aircraft() { float heading_rad = parameters[NAV_OFS_HDG] * Math_PI / 180.0; - Quaternion heading_offset = Quaternion(Vector3(0, 1, 0), heading_rad); - Transform3D offset{Basis{heading_offset}, Vector3{-parameters[NAV_OFS_Y], 0.0, parameters[NAV_OFS_X]}}; + // To rotate clockwise looking from above, that's negative Y in Godot + Quaternion heading_offset = Quaternion(Vector3(0, 1, 0), -heading_rad); + Transform3D offset{Basis{heading_offset}, Vector3{-parameters[NAV_OFS_Y], 0.0, + parameters[NAV_OFS_X]}}; Transform3D marsh{Basis{last_rotation}, last_location}; return offset * marsh; } @@ -320,7 +327,95 @@ void MarshConnector::handle_attitude(mavlink_message_t message) { } void MarshConnector::handle_param(mavlink_message_t message) { - print_line("Implement handle_param"); + uint8_t target_system = 0; + uint8_t target_component = 0; + String param_id; + int16_t param_index = -1; + std::optional param_value; + + if (message.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { + mavlink_param_request_list_t param_request_list; + mavlink_msg_param_request_list_decode(&message, ¶m_request_list); + target_system = param_request_list.target_system; + target_component = param_request_list.target_component; + param_index = -2; // Not used by MAVLink, don't search for name + } else if (message.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { + mavlink_param_request_read_t param_request_read; + mavlink_msg_param_request_read_decode(&message, ¶m_request_read); + target_system = param_request_read.target_system; + target_component = param_request_read.target_component; + param_index = param_request_read.param_index; + if (param_index == -1) { + param_id = String{param_request_read.param_id}; + } + } else if (message.msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t param_set; + mavlink_msg_param_set_decode(&message, ¶m_set); + target_system = param_set.target_system; + target_component = param_set.target_component; + param_id = String{param_set.param_id}; + if (param_set.param_type == MAV_PARAM_EXT_TYPE_REAL32) { + // Only support float parameters, like MARSH Manager and ArduPilot + param_value = param_set.param_value; + } + } + + if (target_system != system_id && target_system != 0) { + return; // Not for us and not broadcast + } + if (target_component != component_id && target_component != 0) { + return; // Not for us and not broadcast + } + if (param_index == -1) { + for (uint16_t i = 0; i < PARAM_COUNT; i++) { + if (parameter_names[i] == param_id) { + param_index = i; + break; + } + } + if (param_index == -1) { + return; // Didn't find parameter + } + } + + if (message.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { + for (uint16_t i = 0; i < PARAM_COUNT; i++) { + send_param(Parameter{i}); + } + return; // Done everything for this message + } + + if (param_index < 0 || param_index >= PARAM_COUNT) { + return; // Invalid param_index + } + uint16_t i = param_index; + + if (param_value.has_value()) { + // TODO: Verify the value before updating + parameters[i] = *param_value; + } + send_param(Parameter{i}); +} + +Error MarshConnector::send_param(Parameter index) { + if (index >= PARAM_COUNT) { + return ERR_PARAMETER_RANGE_ERROR; // "parameter" here as function argument + } + + mavlink_param_value_t param_value; + memset(param_value.param_id, 0, sizeof(param_value.param_id)); + CharString nameBuffer = parameter_names[index].ascii(); + const char *name = nameBuffer.get_data(); + strcpy_s(param_value.param_id, name); + param_value.param_value = parameters[index]; + param_value.param_type = MAV_PARAM_TYPE_REAL32; + param_value.param_count = PARAM_COUNT; + param_value.param_index = index; + + mavlink_message_t message_sent; + mavlink_msg_param_value_encode_chan(system_id, component_id, MAVLINK_COMM_0, + &message_sent, ¶m_value); + return send_message(message_sent); } void MarshConnector::handle_manual_control(mavlink_message_t message) { diff --git a/src/marshconnector.h b/src/marshconnector.h index 029f831..897300a 100644 --- a/src/marshconnector.h +++ b/src/marshconnector.h @@ -87,7 +87,7 @@ private: // 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 { + enum Parameter : uint16_t { // FOG_DENSITY, NAV_OFS_HDG, NAV_OFS_X, @@ -96,12 +96,13 @@ private: LOCAL_FRAME_LON, PARAM_COUNT, }; - float parameters[PARAM_COUNT] = {90.0, -100.0, 300.0, 0.0, 0.0}; + float parameters[PARAM_COUNT] = {0.0, 0.0, 0.0, 0.0, 0.0}; const String parameter_names[PARAM_COUNT] = { // "FOG_DENSITY", "NAV_OFS_HDG", "NAV_OFS_X", "NAV_OFS_Y", "LOCAL_FRAME_LAT", "LOCAL_FRAME_LON", }; + Error send_param(Parameter index); uint8_t system_id; uint8_t component_id;