AP_Networking: added support for mapping network ports

only UDP_CLIENT so far

libraries/AP_Networking/AP_Networking.cpp

@@ -140,6 +140,9 @@ void AP_Networking::init()
 #if AP_NETWORKING_TESTS_ENABLED
     start_tests();
 #endif
+
+    // init network mapped serialmanager ports
+    ports_init();
 }
 
 /*

libraries/AP_Networking/AP_Networking.h

@@ -8,6 +8,8 @@
 
 #include "AP_Networking_address.h"
 #include "AP_Networking_Backend.h"
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_HAL/utility/RingBuffer.h>
 
 /*
   Note! all uint32_t IPv4 addresses are in host byte order
@@ -17,11 +19,14 @@
 class AP_Networking_Backend;
 class AP_Networking_ChibiOS;
 
+class SocketAPM;
+
 class AP_Networking
 {
 public:
     friend class AP_Networking_Backend;
     friend class AP_Networking_ChibiOS;
+    friend class AP_Vehicle;
 
     AP_Networking();
 
@@ -175,6 +180,51 @@ private:
 
     HAL_Semaphore sem;
 
+    enum class NetworkPortType {
+        NONE = 0,
+        UDP_CLIENT = 1,
+    };
+
+    // class for NET_Pn_* parameters
+    class Port : public AP_SerialManager::RegisteredPort {
+    public:
+        /* Do not allow copies */
+        CLASS_NO_COPY(Port);
+
+        Port() {}
+
+        static const struct AP_Param::GroupInfo var_info[];
+        AP_Enum<NetworkPortType> type;
+        AP_Networking_IPV4 ip {"0.0.0.0"};
+        AP_Int32 port;
+        SocketAPM *sock;
+
+        bool is_initialized() override {
+            return true;
+        }
+        bool tx_pending() override {
+            return false;
+        }
+
+        void udp_client_init(void);
+        void udp_client_loop(void);
+
+    private:
+        bool init_buffers(uint32_t size);
+
+        uint32_t txspace() override;
+        void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override;
+        size_t _write(const uint8_t *buffer, size_t size) override;
+        ssize_t _read(uint8_t *buffer, uint16_t count) override;
+        uint32_t _available() override;
+        void _end() override {}
+        void _flush() override {}
+        bool _discard_input() override;
+
+        ByteBuffer *readbuffer;
+        ByteBuffer *writebuffer;
+    };
+
 private:
     uint32_t announce_ms;
 
@@ -187,6 +237,11 @@ private:
     void test_UDP_client(void);
     void test_TCP_client(void);
 #endif // AP_NETWORKING_TESTS_ENABLED
+
+    // ports for registration with serial manager
+    Port ports[AP_NETWORKING_NUM_PORTS];
+
+    void ports_init(void);
 };
 
 namespace AP

libraries/AP_Networking/AP_Networking_Config.h

@@ -74,3 +74,7 @@
 #define AP_NETWORKING_TEST_IP "192.168.13.2"
 #endif
 #endif
+
+#ifndef AP_NETWORKING_NUM_PORTS
+#define AP_NETWORKING_NUM_PORTS 4
+#endif

libraries/AP_Networking/AP_Networking_port.cpp

@@ -0,0 +1,181 @@
+/*
+  class for networking mapped ports
+ */
+
+#include "AP_Networking_Config.h"
+
+#if AP_NETWORKING_ENABLED
+
+#include "AP_Networking.h"
+#include <AP_HAL/utility/Socket.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_Math/AP_Math.h>
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AP_Networking::Port::var_info[] = {
+    // @Param: TYPE
+    // @DisplayName: Port type
+    // @Description: Port type
+    // @Values: 0:Disabled, 1:UDP client
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("TYPE", 1,  AP_Networking::Port, type, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: PROTOCOL
+    // @DisplayName: protocol
+    // @Description: protocol
+    // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:Gimbal, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:EFI Serial, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency, 44:IRC Tramp
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("PROTOCOL", 2,  AP_Networking::Port, state.protocol, 0),
+
+    // @Group: IP
+    // @Path: AP_Networking_address.cpp
+    AP_SUBGROUPINFO(ip, "IP", 3,  AP_Networking::Port, AP_Networking_IPV4),
+
+    // @Param: PORT
+    // @DisplayName: Port number
+    // @Description: Port number
+    // @Range: 0 65535
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("PORT", 4,  AP_Networking::Port, port, 0),
+    
+    AP_GROUPEND
+};
+
+/*
+  initialise mapped network ports
+ */
+void AP_Networking::ports_init(void)
+{
+    // init in reverse order to keep the linked list in
+    // AP_SerialManager in the right order
+    for (int8_t i=ARRAY_SIZE(ports)-1; i>= 0; i--) {
+        auto &p = ports[i];
+        NetworkPortType ptype = (NetworkPortType)p.type;
+        p.state.idx = AP_SERIALMANAGER_NET_PORT_1 + i;
+        switch (ptype) {
+        case NetworkPortType::NONE:
+            break;
+        case NetworkPortType::UDP_CLIENT:
+            p.udp_client_init();
+            break;
+        }
+        if (p.sock != nullptr) {
+            AP::serialmanager().register_port(&p);
+        }
+    }
+}
+
+/*
+  initialise a UDP client
+ */
+void AP_Networking::Port::udp_client_init(void)
+{
+    if (!init_buffers(4096)) {
+        return;
+    }
+    sock = new SocketAPM(true);
+    if (sock == nullptr) {
+        return;
+    }
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_client_loop, void), "NET", 2048, AP_HAL::Scheduler::PRIORITY_UART, 0)) {
+        AP_BoardConfig::allocation_error("Failed to allocate UDP client thread");
+    }
+}
+
+/*
+  update a UDP client
+ */
+void AP_Networking::Port::udp_client_loop(void)
+{
+    while (!hal.scheduler->is_system_initialized()) {
+        hal.scheduler->delay(100);
+    }
+    hal.scheduler->delay(1000);
+
+    const char *dest = ip.get_str();
+    if (!sock->connect(dest, port.get())) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to connect to %s", (unsigned)state.idx, dest);
+        delete sock;
+        sock = nullptr;
+        return;
+    }
+
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", (unsigned)state.idx, dest, unsigned(port.get()));
+
+    while (true) {
+        hal.scheduler->delay_microseconds(500);
+
+        // handle outgoing packets
+        uint32_t available;
+        const auto *ptr = writebuffer->readptr(available);
+        if (ptr != nullptr) {
+            const auto ret = sock->send(ptr, available);
+            if (ret > 0) {
+                writebuffer->advance(ret);
+            }
+        }
+
+        // handle incoming packets
+        const auto space = readbuffer->space();
+        if (space > 0) {
+            const uint32_t n = MIN(350U, space);
+            uint8_t buf[n];
+            const auto ret = sock->recv(buf, n, 0);
+            if (ret > 0) {
+                readbuffer->write(buf, ret);
+            }
+        }
+    }
+}
+
+/*
+  available space in outgoing buffer
+ */
+uint32_t AP_Networking::Port::txspace(void)
+{
+    return writebuffer->space();
+}
+
+void AP_Networking::Port::_begin(uint32_t b, uint16_t rxS, uint16_t txS)
+{
+    // TODO: use buffer sizes
+}
+
+size_t AP_Networking::Port::_write(const uint8_t *buffer, size_t size)
+{
+    return writebuffer->write(buffer, size);
+}
+
+ssize_t AP_Networking::Port::_read(uint8_t *buffer, uint16_t count)
+{
+    return readbuffer->read(buffer, count);
+}
+
+uint32_t AP_Networking::Port::_available()
+{
+    return readbuffer->available();
+}
+
+
+bool AP_Networking::Port::_discard_input()
+{
+    readbuffer->clear();
+    return true;
+}
+
+/*
+  initialise read/write buffers
+ */
+bool AP_Networking::Port::init_buffers(uint32_t size)
+{
+    readbuffer = new ByteBuffer(size);
+    writebuffer = new ByteBuffer(size);
+    return readbuffer != nullptr && writebuffer != nullptr;
+}
+
+#endif // AP_NETWORKING_ENABLED