Handle parameters over MAVLink and document their usage

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?

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 <cstdint>
+#include <optional>
 
 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<float> 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, &param_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, &param_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, &param_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, &param_value);
+  return send_message(message_sent);
 }
 
 void MarshConnector::handle_manual_control(mavlink_message_t message) {

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;