custom storage feature added to Plane:4.6.2

Tools/ardupilotwaf/boards.py

@@ -178,6 +178,25 @@ def srcpath(path):
         if cfg.options.enable_networking_tests:
             env.CXXFLAGS += ['-DAP_NETWORKING_TESTS_ENABLED=1']
 
+        if cfg.options.enable_custom_storage:
+            # Set environment variable to make the flag globally available
+            env.ENABLE_CUSTOM_STORAGE = True
+
+            # Add necessary libraries to compile when the flag is present
+            env.AP_LIBRARIES += [
+                'AP_CustomStorage',
+                'AP_CustomMavlinkHandler'
+            ]
+
+            # Define a preprocessor macro that can be checked in C/C++ code
+            env.DEFINES.update(AP_ENABLE_CUSTOM_STORAGE = 1)
+        else:
+            # Ensure the environment variable is set to False when the flag is not present
+            env.ENABLE_CUSTOM_STORAGE = False
+
+            # Set the preprocessor macro to 0 when disabled
+            env.DEFINES.update(AP_ENABLE_CUSTOM_STORAGE = 0)
+
         d = env.get_merged_dict()
         # Always prepend so that arguments passed in the command line get
         # the priority.

Tools/ardupilotwaf/custom_dialect_tool.py

@@ -0,0 +1,67 @@
+import os
+
+# All Waf tools must have a 'configure' function that Waf can call.
+def configure(conf):
+    """
+    This function is automatically run by Waf when 'cfg.load("custom_dialect_tool")'
+    is called in the main wscript.
+    """
+    # Use 'conf' which is the configuration context passed into this tool.
+    conf.to_log("Running custom_dialect_tool to manage MAVLink dialect...")
+
+    # Dynamically get the absolute path to the ardupilot root directory
+    ardupilot_root = conf.srcnode.abspath()
+
+    # Construct the full path to your custom dialect file
+    custom_dialect_path = os.path.join(ardupilot_root, 'libraries/AP_CustomMavlinkHandler/custom_dialect.xml')
+
+    # The path to the all.xml file within the ArduPilot source tree
+    all_xml_path = os.path.join(ardupilot_root, 'modules/mavlink/message_definitions/v1.0/all.xml')
+
+    try:
+        with open(all_xml_path, 'r') as f:
+            original_lines = f.readlines()
+    except FileNotFoundError:
+        conf.fatal(f"Could not find {all_xml_path}. Make sure submodules are updated.")
+    except Exception as e:
+        conf.fatal(f"An error occurred while reading {all_xml_path}: {e}")
+
+    # Check if the custom line currently exists
+    line_exists = any(custom_dialect_path in line for line in original_lines)
+    needs_write = False
+
+    if conf.options.enable_custom_storage:
+        # Flag is ON: We want the line to EXIST
+        if not line_exists:
+            insert_index = -1
+            for i, line in enumerate(original_lines):
+                if '<messages/>' in line:
+                    insert_index = i
+                    break
+
+            if insert_index == -1:
+                conf.fatal(f"Could not find insertion point (<messages/>) in {all_xml_path}")
+
+            include_line_to_add = f'  <include>{custom_dialect_path}</include>\n'
+            original_lines.insert(insert_index, include_line_to_add)
+            needs_write = True
+            conf.to_log(f"-> Adding custom dialect to {all_xml_path}")
+        print('Custom Storage Setting                         : Enabled')
+
+    else:
+        # Flag is OFF: We want the line to be REMOVED
+        if line_exists:
+            original_lines = [line for line in original_lines if custom_dialect_path not in line]
+            needs_write = True
+            conf.to_log(f"-> Removing custom dialect from {all_xml_path}")
+        print('Custom Storage Setting                         : Disabled')
+
+    # If a change was made, write the new content back to the file
+    if needs_write:
+        try:
+            with open(all_xml_path, 'w') as f:
+                f.writelines(original_lines)
+        except Exception as e:
+            conf.fatal(f"An error occurred while writing to {all_xml_path}: {e}")
+    else:
+        conf.to_log(f"-> Custom dialect in {all_xml_path} is already in the correct state.")

Tools/autotest/sim_vehicle.py

@@ -420,6 +420,9 @@ def do_build(opts, frame_options):
     if opts.enable_networking_tests:
         cmd_configure.append("--enable-networking-tests")
 
+    if opts.enable_custom_storage:
+        cmd_configure.append("--enable-custom-storage")
+
     pieces = [shlex.split(x) for x in opts.waf_configure_args]
     for piece in pieces:
         cmd_configure.extend(piece)
@@ -1355,6 +1358,8 @@ def generate_frame_help():
                      help="Enable networking tests")
 group_sim.add_option("--enable-fgview", action='store_true',
                      help="Enable FlightGear output")
+group_sim.add_option("--enable-custom-storage", action='store_true',
+                     help="Enable custom storage for uuid and password along with custom mavlink message handler.")
 
 parser.add_option_group(group_sim)
 

libraries/AP_CustomMavlinkHandler/AP_CustomMavlinkHandler.cpp

@@ -0,0 +1,87 @@
+#if defined(AP_ENABLE_CUSTOM_STORAGE) && AP_ENABLE_CUSTOM_STORAGE==1
+
+#include "AP_CustomMavlinkHandler.h"
+#include <string.h>
+#include <stdio.h>
+
+void AP_CustomMavlinkHandler::init(void){
+    g_custom_storage.init();
+}
+void AP_CustomMavlinkHandler::handle_custom_message(mavlink_channel_t chan, const mavlink_message_t &msg)
+{
+    // printf("AP_CustomMavlinkHandler: %s\n", "got message");
+    if (msg.msgid != CUSTOM_MSG_ID)
+        return;
+
+    // Manual message decoding
+    uuid_update_t packet;
+    memcpy(&packet, msg.payload64, sizeof(packet));
+    packet.value[36] = '\0'; // Ensure null termination
+    // printf("payload64: %s\n", packet.value);
+
+    switch (packet.param)
+    {
+    case AIRBOUND_PARAMETER_PARAM_ID_UUID:
+        switch (packet.action)
+        {
+        case AIRBOUND_PARAMETER_ACTION_GET: // read
+        {
+            char uuid[37] = {0};
+            g_custom_storage.get_uuid(uuid, sizeof(uuid));
+            gcs().send_text(MAV_SEVERITY_INFO, "uuid:%s", uuid);
+            mavlink_msg_airbound_parameter_status_send(chan, AIRBOUND_PARAMETER_PARAM_ID_UUID, (const char *)uuid, AIRBOUND_PARAMETER_RESULT_OK);
+            break;
+        }
+        case AIRBOUND_PARAMETER_ACTION_SET: // write
+        {
+            char uuid[37] = {0};
+            g_custom_storage.set_uuid(packet.value);
+            g_custom_storage.get_uuid(uuid, sizeof(uuid));
+            gcs().send_text(MAV_SEVERITY_INFO, "uuid updated : %s", uuid);
+            mavlink_msg_airbound_parameter_status_send(chan, AIRBOUND_PARAMETER_PARAM_ID_UUID, (const char *)uuid, AIRBOUND_PARAMETER_RESULT_OK);
+
+            break;
+        }
+        default:
+            char buf[37] = {0};
+            mavlink_msg_airbound_parameter_status_send(chan, AIRBOUND_PARAMETER_PARAM_ID_UUID, (const char *)buf, AIRBOUND_PARAMETER_RESULT_UNSUPPORTED);
+            break;
+        }
+        break; 
+
+    case AIRBOUND_PARAMETER_PARAM_ID_PASS:
+        switch (packet.action)
+        {
+        case AIRBOUND_PARAMETER_ACTION_GET: // read
+        {
+            char pass[37] = {0};
+            g_custom_storage.get_password(pass, sizeof(pass));
+            mavlink_msg_airbound_parameter_status_send(chan, AIRBOUND_PARAMETER_PARAM_ID_PASS, (const char *)pass, AIRBOUND_PARAMETER_RESULT_OK);
+            break;
+        }
+        case AIRBOUND_PARAMETER_ACTION_SET: // write
+        {
+            char pass[37] = {0};
+            g_custom_storage.set_password(packet.value);
+            g_custom_storage.get_password(pass, sizeof(pass));
+            mavlink_msg_airbound_parameter_status_send(chan, AIRBOUND_PARAMETER_PARAM_ID_PASS, (const char *)pass, AIRBOUND_PARAMETER_RESULT_OK);
+            break;
+        }
+        default:
+        {
+            char buf[37] = {0};
+            mavlink_msg_airbound_parameter_status_send(chan, AIRBOUND_PARAMETER_PARAM_ID_PASS, (const char *)buf, AIRBOUND_PARAMETER_RESULT_UNSUPPORTED);
+            break;
+        }
+        }
+        break;
+
+    default:
+    {
+        char buf[37] = {0};
+        mavlink_msg_airbound_parameter_status_send(chan, AIRBOUND_PARAMETER_PARAM_ID_PASS, (const char *)buf, AIRBOUND_PARAMETER_RESULT_UNSUPPORTED);
+        break;
+    }
+    }
+}
+#endif

libraries/AP_CustomMavlinkHandler/AP_CustomMavlinkHandler.h

@@ -0,0 +1,24 @@
+#if defined(AP_ENABLE_CUSTOM_STORAGE) && AP_ENABLE_CUSTOM_STORAGE==1
+
+#pragma once
+#include <GCS_MAVLink/GCS.h>
+#include "AP_CustomStorage/AP_CustomStorage.h"
+
+class AP_CustomMavlinkHandler
+{
+public:
+    static void handle_custom_message(mavlink_channel_t chan, const mavlink_message_t &msg);
+    static void init(void);
+// Manually define our message structure
+#pragma pack(push, 1)
+    typedef struct
+    {
+        uint8_t param;
+        uint8_t action;
+        char value[37];
+    } uuid_update_t;
+#pragma pack(pop)
+    static const uint16_t CUSTOM_MSG_ID = 15222;
+};
+
+#endif

libraries/AP_CustomMavlinkHandler/custom_dialect.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<mavlink>
+  <dialect>2</dialect>
+  <enums>
+    <!-- custom messages enums START -->
+    <enum name="AIRBOUND_PARAMETER_ACTION_TYPE">
+      <entry value="0" name="AIRBOUND_PARAMETER_ACTION_GET"/>
+      <entry value="1" name="AIRBOUND_PARAMETER_ACTION_SET"/>
+    </enum>
+    <enum name="AIRBOUND_PARAMETER_PARAM_ID">
+      <entry value="1" name="AIRBOUND_PARAMETER_PARAM_ID_UUID"/>
+      <entry value="2" name="AIRBOUND_PARAMETER_PARAM_ID_PASS"/>
+    </enum>
+    <enum name="AIRBOUND_PARAMETER_RESULT">
+      <entry value="0" name="AIRBOUND_PARAMETER_RESULT_OK"/>
+      <entry value="1" name="AIRBOUND_PARAMETER_RESULT_FAILED"/>
+      <entry value="2" name="AIRBOUND_PARAMETER_RESULT_UNSUPPORTED"/>
+    </enum>
+    <!-- custom messages enums END -->
+  </enums>
+  <messages>
+    <!-- custom messages START -->
+    <message id="15222" name="AIRBOUND_PARAMETER_GETSET">
+      <description>Custom Airbound Parameter GET SET API for setting or getting a simple parameter.</description>
+      <field type="uint8_t" name="param_id" enum="AIRBOUND_PARAMETER_PARAM_ID">Identifier for the custom parameter.</field>
+      <field type="uint8_t" name="action_type" enum="AIRBOUND_PARAMETER_ACTION_TYPE">Action to perform (GET or SET).</field>
+      <field type="char[37]" name="data_value">Value to set (ignored for GET action).</field>
+    </message>
+    <message id="15223" name="AIRBOUND_PARAMETER_STATUS">
+      <description>Status and value feedback for a custom Airbound parameter.</description>
+      <field name="param_id" type="uint8_t">Identifier for the custom parameter from the request.</field>
+      <field name="current_value" type="char[37]">The current value of the parameter.</field>
+      <field name="result" type="uint8_t" enum="AIRBOUND_PARAMETER_RESULT">Result of the operation.</field>
+    </message>
+    <!-- custom messages END -->
+  </messages>
+</mavlink>