support SECURE_COMMAND SET_CONFIG

ArduPilot · Sep 18, 2022 · f0eb204 · f0eb204
1 parent 6cc305f
commit f0eb204
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 1 deletion.
diff --git a/RemoteIDModule/mavlink.cpp b/RemoteIDModule/mavlink.cpp
@@ -42,6 +42,7 @@ void MAVLinkSerial::init(void)
     // print banner at startup
     serial.printf("ArduRemoteID version %u.%u %08x\n",
                   FW_VERSION_MAJOR, FW_VERSION_MINOR, GIT_VERSION);
+    mavlink_system.sysid = g.mavlink_sysid;
 }
 
 void MAVLinkSerial::update(void)
@@ -50,6 +51,8 @@ void MAVLinkSerial::update(void)
 
     if (mavlink_system.sysid != 0) {
         update_send();
+    } else if (g.mavlink_sysid != 0) {
+        mavlink_system.sysid = g.mavlink_sysid;
     } else if (now_ms - last_hb_warn_ms >= 2000) {
         last_hb_warn_ms = millis();
         serial.printf("Waiting for heartbeat\n");

diff --git a/RemoteIDModule/mavlink_secure_command.cpp b/RemoteIDModule/mavlink_secure_command.cpp
@@ -28,7 +28,8 @@ void MAVLinkSerial::handle_secure_command(const mavlink_secure_command_t &pkt)
 
     switch (pkt.operation) {
 
-    case SECURE_COMMAND_GET_SESSION_KEY: {
+    case SECURE_COMMAND_GET_SESSION_KEY:
+    case SECURE_COMMAND_GET_REMOTEID_SESSION_KEY: {
         make_session_key(session_key);
         reply.data_length = sizeof(session_key);
         memcpy(reply.data, session_key, reply.data_length);
@@ -108,6 +109,33 @@ void MAVLinkSerial::handle_secure_command(const mavlink_secure_command_t &pkt)
         reply.result = MAV_RESULT_ACCEPTED;
         break;
     }
+    case SECURE_COMMAND_SET_REMOTEID_CONFIG: {
+        int16_t data_len = pkt.data_length;
+        char data[pkt.data_length+1];
+        memcpy(data, pkt.data, pkt.data_length);
+        data[pkt.data_length] = 0;
+        /*
+          command buffer is nul separated set of NAME=VALUE pairs
+         */
+        reply.result = MAV_RESULT_ACCEPTED;
+        char *command = (char *)data;
+        while (data_len > 0) {
+            uint8_t cmdlen = strlen(command);
+            char *eq = strchr(command, '=');
+            if (eq != nullptr) {
+                *eq = 0;
+                if (!g.set_by_name_string(command, eq+1)) {
+                    mav_printf(MAV_SEVERITY_INFO, "set %s failed", command);
+                    reply.result = MAV_RESULT_FAILED;
+                } else {
+                    mav_printf(MAV_SEVERITY_INFO, "set %s OK", command);
+                }
+            }
+            command += cmdlen+1;
+            data_len -= cmdlen+1;
+        }
+        break;
+    }
     }
 
 send_reply:

diff --git a/RemoteIDModule/parameters.cpp b/RemoteIDModule/parameters.cpp
@@ -31,6 +31,7 @@ const Parameters::Param Parameters::params[] = {
     { "PUBLIC_KEY3",       Parameters::ParamType::CHAR64, (const void*)&g.public_keys[2], },
     { "PUBLIC_KEY4",       Parameters::ParamType::CHAR64, (const void*)&g.public_keys[3], },
     { "PUBLIC_KEY5",       Parameters::ParamType::CHAR64, (const void*)&g.public_keys[4], },
+    { "MAVLINK_SYSID",     Parameters::ParamType::UINT8,  (const void*)&g.mavlink_sysid,    0, 0, 254 },
     { "DONE_INIT",         Parameters::ParamType::UINT8,  (const void*)&g.done_init,        0, 0, 0, PARAM_FLAG_HIDDEN},
     { "",                  Parameters::ParamType::NONE,   nullptr,  },
 };

diff --git a/RemoteIDModule/parameters.h b/RemoteIDModule/parameters.h
@@ -27,6 +27,7 @@ class Parameters {
     float bt5_power;
     uint8_t done_init;
     uint8_t webserver_enable;
+    uint8_t mavlink_sysid;
     char wifi_ssid[21] = "";
     char wifi_password[21] = "ArduRemoteID";
     struct {