AP_Periph: add Networking-UART passthrough

ArduPilot · Nov 22, 2023 · a4b8d71 · a4b8d71
1 parent 9032b04
commit a4b8d71
Show file tree

Hide file tree

Showing 6 changed files with 286 additions and 6 deletions.
diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
@@ -101,7 +101,7 @@ void AP_Periph_FW::init()
     can_start();
 
 #ifdef HAL_PERIPH_ENABLE_NETWORKING
-    networking.init();
+    networking_periph.init();
 #endif
 
 #if HAL_GCS_ENABLED
@@ -502,7 +502,7 @@ void AP_Periph_FW::update()
     can_update();
 
 #ifdef HAL_PERIPH_ENABLE_NETWORKING
-    networking.update();
+    networking_periph.update();
 #endif
 
 #if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
@@ -36,6 +36,7 @@
 #include <AP_RCProtocol/AP_RCProtocol_config.h>
 #include "rc_in.h"
 #include "batt_balance.h"
+#include "networking.h"
 
 #include <AP_NMEA_Output/AP_NMEA_Output.h>
 #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS))
@@ -356,7 +357,7 @@ class AP_Periph_FW {
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_NETWORKING
-    AP_Networking networking;
+    Networking_Periph networking_periph;
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RTC

diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
@@ -566,8 +566,8 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
 
 #ifdef HAL_PERIPH_ENABLE_NETWORKING
     // @Group: NET_
-    // @Path: ../libraries/AP_Networking/AP_Networking.cpp
-    GOBJECT(networking, "NET_", AP_Networking),
+    // @Path: networking.cpp
+    GOBJECT(networking_periph, "NET_", Networking_Periph),
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RPM

diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
@@ -76,7 +76,7 @@ class Parameters {
         k_param_esc_number1,
         k_param_pole_count1,
         k_param_esc_serial_port1,
-        k_param_networking,
+        k_param_networking_periph,
         k_param_rpm_sensor,
         k_param_g_rcin,
         k_param_sitl,

diff --git a/Tools/AP_Periph/networking.cpp b/Tools/AP_Periph/networking.cpp
@@ -0,0 +1,223 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_Periph.h"
+
+#ifdef HAL_PERIPH_ENABLE_NETWORKING
+
+#include <AP_SerialManager/AP_SerialManager.h>
+
+extern const AP_HAL::HAL &hal;
+
+const AP_Param::GroupInfo Networking_Periph::var_info[] {
+    // @Group:
+    // @Path: ../AP_Networking/AP_Networking.cpp
+    AP_SUBGROUPINFO(networking_lib, "", 1, Networking_Periph, AP_Networking),
+
+    /*
+      the NET_Pn_ parameters need to be bere as otherwise we
+      are too deep in the parameter tree
+     */
+
+#if AP_NETWORKING_NUM_PORTS > 0
+    // @Group: P1_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[0], "P1_", 2, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 1
+    // @Group: P2_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[1], "P2_", 3, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 2
+    // @Group: P3_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[2], "P3_", 4, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 3
+    // @Group: P4_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[3], "P4_", 5, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 4
+    // @Group: P5_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[4], "P5_", 6, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 5
+    // @Group: P6_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[5], "P6_", 7, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 6
+    // @Group: P7_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[6], "P7_", 8, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 7
+    // @Group: P8_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[7], "P8_", 9, Networking_Periph, AP_Networking::Port),
+#endif
+
+#if AP_NETWORKING_NUM_PORTS > 8
+    // @Group: P9_
+    // @Path: ../libraries/AP_Networking/AP_Networking_port.cpp
+    AP_SUBGROUPINFO(networking_lib.ports[8], "P9_", 10, Networking_Periph, AP_Networking::Port),
+#endif
+
+
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0
+    // @Group: PASS1_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[0], "PASS1_", 11, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 1
+    // @Group: PASS2_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[1], "PASS2_", 12, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 2
+    // @Group: PASS3_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[2], "PASS3_", 13, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 3
+    // @Group: PASS4_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[3], "PASS4_", 14, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 4
+    // @Group: PASS5_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[4], "PASS5_", 15, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 5
+    // @Group: PASS6_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[5], "PASS6_", 16, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 6
+    // @Group: PASS7_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[6], "PASS7_", 17, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 7
+    // @Group: PASS8_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[7], "PASS8_", 18, Networking_Periph, Passthru),
+#endif
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 8
+    // @Group: PASS9_
+    // @Path: networking.cpp
+    AP_SUBGROUPINFO(passthru[8], "PASS9_", 19, Networking_Periph, Passthru),
+#endif
+
+    AP_GROUPEND
+};
+
+
+const AP_Param::GroupInfo Networking_Periph::Passthru::var_info[] = {
+    AP_GROUPINFO_FLAGS("ENABLE", 1,  Networking_Periph::Passthru, enabled, 0, AP_PARAM_FLAG_ENABLE),
+    AP_GROUPINFO("EP1", 2,  Networking_Periph::Passthru, ep1, -1),
+    AP_GROUPINFO("EP2", 3,  Networking_Periph::Passthru, ep2, -1),
+    AP_GROUPINFO("BAUD1", 4,  Networking_Periph::Passthru, baud1, 115200),
+    AP_GROUPINFO("BAUD2", 5,  Networking_Periph::Passthru, baud2, 115200),
+    AP_GROUPINFO("OPT1", 6,  Networking_Periph::Passthru, options1, 0),
+    AP_GROUPINFO("OPT2", 7,  Networking_Periph::Passthru, options2, 0),
+
+    AP_GROUPEND
+};
+
+void Networking_Periph::init(void)
+{
+    networking_lib.init();
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0
+    auto &serial_manager = AP::serialmanager();
+
+    for (auto &p : passthru) {
+        if (p.enabled != 0 && p.port1 == nullptr && p.port2 == nullptr &&
+            p.ep1 != -1 && p.ep2 != -1 && p.ep1 != p.ep2) {
+
+            p.port1 = serial_manager.get_serial_by_id(p.ep1);
+            p.port2 = serial_manager.get_serial_by_id(p.ep2);
+
+            if (p.port1 != nullptr && p.port2 != nullptr) {
+                p.port1->set_options(p.options1);
+                p.port1->begin(p.baud1);
+
+                p.port2->set_options(p.options2);
+                p.port2->begin(p.baud2);
+            }
+        }
+    }
+#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU
+}
+
+void Networking_Periph::update(void)
+{
+    networking_lib.update();
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0
+    for (auto &p : passthru) {
+        if (p.enabled == 0 || p.port1 == nullptr || p.port2 == nullptr) {
+            continue;
+        }
+        uint8_t buf[1024];
+
+        // read from port1, and write to port2
+        auto avail = p.port1->available();
+        if (avail > 0) {
+            auto space = p.port2->txspace();
+            const uint32_t n = MIN(space, sizeof(buf));
+            const auto nbytes = p.port1->read(buf, n);
+            if (nbytes > 0) {
+                p.port2->write(buf, nbytes);
+            }
+        }
+
+        // read from port2, and write to port1
+        avail = p.port2->available();
+        if (avail > 0) {
+            auto space = p.port1->txspace();
+            const uint32_t n = MIN(space, sizeof(buf));
+            const auto nbytes = p.port2->read(buf, n);
+            if (nbytes > 0) {
+                p.port1->write(buf, nbytes);
+            }
+        }
+    }
+#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU
+}
+
+#endif  // HAL_PERIPH_ENABLE_NETWORKING
+
diff --git a/Tools/AP_Periph/networking.h b/Tools/AP_Periph/networking.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#ifdef HAL_PERIPH_ENABLE_NETWORKING
+
+#ifndef HAL_PERIPH_NETWORK_NUM_PASSTHRU
+#define HAL_PERIPH_NETWORK_NUM_PASSTHRU 2
+#endif
+
+class Networking_Periph {
+public:
+    friend class AP_Periph_FW;
+    friend class AP_Networking;
+
+    Networking_Periph() {
+            AP_Param::setup_object_defaults(this, var_info);
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+    void init();
+    void update();
+
+private:
+
+#if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0
+    class Passthru {
+    public:
+        friend class Networking_Periph;
+
+        CLASS_NO_COPY(Passthru);
+
+        Passthru() {
+            AP_Param::setup_object_defaults(this, var_info);
+        }
+
+        static const struct AP_Param::GroupInfo var_info[];
+
+    private:
+        AP_Int8 enabled;
+        AP_Int8 ep1;
+        AP_Int8 ep2;
+        AP_Int32 baud1;
+        AP_Int32 baud2;
+        AP_Int32 options1;
+        AP_Int32 options2;
+
+        AP_HAL::UARTDriver *port1;
+        AP_HAL::UARTDriver *port2;
+    } passthru[HAL_PERIPH_NETWORK_NUM_PASSTHRU];
+#endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU
+
+    AP_Networking networking_lib;
+};
+
+#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE
+