SITL support for UDP connected network peripherals

Tools/autotest/arducopter.py

@@ -8494,6 +8494,44 @@ def MountTopotekNetwork(self):
         )
         self.wait_mount_roll_pitch_yaw_deg(p=-30)
 
+    def MountTopotekNetworkUDP(self):
+        '''test Topotek gimbal connected via a UDP network port rather than TCP
+
+        deliberately the same body as MountTopotekNetwork() other than the
+        transport - this is a regression guard for create_net_serial_sim()'s
+        UDP support and its NAME:PORT,PROTOCOL spec format, not a Topotek-specific
+        test'''
+        self.set_parameters({
+            "MNT1_TYPE": 12,      # Topotek
+            "CAM1_TYPE": 4,       # Mount
+            "NET_ENABLE": 1,
+            "NET_P1_TYPE": 1,     # UDP client
+            "NET_P1_PROTOCOL": 8,  # gimbal
+            "NET_P1_IP0": 127,
+            "NET_P1_IP1": 0,
+            "NET_P1_IP2": 0,
+            "NET_P1_IP3": 1,
+            "NET_P1_PORT": 15007,
+        })
+        # the simulated gimbal listens on a UDP socket rather than a TCP
+        # socket or one of the autopilot's serial ports; the ",udp" suffix
+        # is the new comma-grouped option syntax being tested here:
+        self.customise_SITL_commandline(["--net-device=topotek:15007,udp"])
+        self.mount_check_camera_information(
+            "Topotek", "SIM_TP",
+            expected_fw_version=1,
+            expected_cap_flags=0x6C3,
+        )
+        # command an angle and check the gimbal reports reaching it,
+        # which requires traffic in both directions:
+        self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
+            p1=-30,  # pitch angle in degrees
+            p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,
+        )
+        self.wait_mount_roll_pitch_yaw_deg(p=-30)
+
     def MountViewPro(self):
         '''test Viewpro gimbal using SIM_Viewpro simulator'''
         self.set_parameters({
@@ -20015,6 +20053,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.TakeoffWithLocation,
             self.MountTopotek,
             self.MountTopotekNetwork,
+            self.MountTopotekNetworkUDP,
             self.MountViewPro,
             self.MountAVTCM62,
             self.MountAVTCM62Dual,

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -14,6 +14,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <errno.h>
+#include <strings.h>
 #include <sys/select.h>
 
 #include <AP_Param/AP_Param.h>
@@ -359,10 +360,16 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
 
 #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
 /*
-  create a simulated device which the autopilot connects to over TCP
-  rather than over one of its simulated serial ports.  This is used to
-  simulate devices attached to the autopilot's network ports (NET_Pn).
-  spec is of the form NAME:TCPPORT e.g. "topotek:15005"
+  create a simulated device which the autopilot connects to over the
+  network (TCP by default, or UDP) rather than over one of its
+  simulated serial ports.  This is used to simulate devices attached
+  to the autopilot's network ports (NET_Pn).
+  spec is of the form NAME:PORT or NAME:PORT,OPTION,OPTION,..., e.g.
+  "topotek:15005" (TCP, the default) or "topotek:15005,udp" - any
+  options beyond the port number are comma-separated from each other
+  (and from the port number), rather than each being tacked on with
+  another colon, since they're logically grouped with the port rather
+  than being another NAME-like top-level field
  */
 void SITL_State_Common::create_net_serial_sim(const char *spec)
 {
@@ -376,14 +383,27 @@ void SITL_State_Common::create_net_serial_sim(const char *spec)
     }
     char *saveptr = nullptr;
     const char *name = strtok_r(s, ":", &saveptr);
-    const char *port_str = strtok_r(nullptr, ":", &saveptr);
-    if (name == nullptr || port_str == nullptr) {
-        AP_HAL::panic("Bad network device (%s); expected NAME:TCPPORT", spec);
+    char *port_and_options = strtok_r(nullptr, ":", &saveptr);
+    if (name == nullptr || port_and_options == nullptr) {
+        AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec);
+    }
+    char *saveptr2 = nullptr;
+    const char *port_str = strtok_r(port_and_options, ",", &saveptr2);
+    const char *protocol_str = strtok_r(nullptr, ",", &saveptr2);  // optional, defaults to "tcp"
+    if (port_str == nullptr) {
+        AP_HAL::panic("Bad network device (%s); expected NAME:PORT[,PROTOCOL]", spec);
+    }
+    const bool use_udp = (protocol_str != nullptr) && (strcasecmp(protocol_str, "udp") == 0);
+    if (protocol_str != nullptr && !use_udp && strcasecmp(protocol_str, "tcp") != 0) {
+        AP_HAL::panic("Bad network device protocol (%s); expected 'tcp' or 'udp'", protocol_str);
     }
 
     SITL::SerialDevice *device = create_serial_sim(name, nullptr, 0);
-    if (!device->listen_on_tcp_port(atoi(port_str))) {
-        AP_HAL::panic("Failed to attach %s to TCP port %s", name, port_str);
+    const bool ok = use_udp ?
+        device->listen_on_udp_port(atoi(port_str)) :
+        device->listen_on_tcp_port(atoi(port_str));
+    if (!ok) {
+        AP_HAL::panic("Failed to attach %s to %s port %s", name, use_udp ? "UDP" : "TCP", port_str);
     }
     net_serial_sims[num_net_serial_sims++] = device;
 
@@ -397,8 +417,9 @@ void SITL_State_Common::create_net_serial_sim(const char *spec)
 void SITL_State_Common::sim_update(void)
 {
 #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
-    // move data between the autopilot and any device attached via TCP;
-    // for serially-attached devices the SITL UART driver does this:
+    // move data between the autopilot and any device attached via a
+    // network socket (TCP or UDP); for serially-attached devices the
+    // SITL UART driver does this:
     for (uint8_t i=0; i<num_net_serial_sims; i++) {
         net_serial_sims[i]->network_update();
     }

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -109,7 +109,7 @@ void SITL_State::_usage(void)
            "\t--serial8 device         set device string for SERIAL8\n"
            "\t--serial9 device         set device string for SERIAL9\n"
            "\t--uartA device           alias for --serial0 (do not use)\n"
-           "\t--net-device NAME:PORT   attach simulated device NAME to TCP port PORT rather than to a serial port\n"
+           "\t--net-device NAME:PORT[,udp]   attach simulated device NAME to TCP (or, with ',udp', UDP) port PORT rather than to a serial port\n"
            "\t--base-port PORT         set port num for base port(default 5670) must be before -I option\n"
            "\t--rc-in-port PORT        set port num for rc in\n"
            "\t--sim-address ADDR       set address string for simulator\n"
@@ -267,7 +267,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
     struct AP_Param::defaults_table_struct temp_cmdline_param{};
 
 #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
-    // NAME:TCPPORT strings from --net-device options:
+    // NAME:PORT[,udp] strings from --net-device options:
     const char *net_device_strings[4];
     uint8_t num_net_device_strings = 0;
 #endif  // AP_SIM_SERIALDEVICE_NETWORK_ENABLED

libraries/SITL/SIM_SerialDevice.cpp

@@ -159,10 +159,40 @@ bool SerialDevice::listen_on_tcp_port(const uint16_t port)
         listener = nullptr;
         return false;
     }
+    listener_is_udp = false;
     ::printf("SIM: device listening for autopilot on TCP port %u\n", unsigned(port));
     return true;
 }
 
+/*
+  attach this device to a UDP socket.  The autopilot sends datagrams to
+  this socket (e.g. with a NET_Pn port configured as a UDP client)
+  instead of talking to the device over a simulated serial port.
+  Unlike TCP there is no connection to accept; the autopilot's address
+  is simply learned from whichever packet it last sent us, and replies
+  are sent back to that address
+ */
+bool SerialDevice::listen_on_udp_port(const uint16_t port)
+{
+    listener = NEW_NOTHROW SocketAPM_native(true);
+    if (listener == nullptr) {
+        return false;
+    }
+    listener->reuseaddress();
+    if (!listener->bind("127.0.0.1", port) ||
+        !listener->set_blocking(false)) {
+        ::fprintf(stderr, "SIM: failed to bind UDP port %u: %m\n", unsigned(port));
+        delete listener;
+        listener = nullptr;
+        return false;
+    }
+    listener_is_udp = true;
+    udp_peer_addr = 0;
+    udp_peer_port = 0;
+    ::printf("SIM: device listening for autopilot on UDP port %u\n", unsigned(port));
+    return true;
+}
+
 /*
   move bytes between the network socket and this device.  This performs
   the same role the SITL UART driver performs for serially-attached
@@ -175,6 +205,11 @@ void SerialDevice::network_update()
         return;
     }
 
+    if (listener_is_udp) {
+        network_update_udp();
+        return;
+    }
+
     if (sock == nullptr) {
         sock = listener->accept(0);
         if (sock == nullptr) {
@@ -212,6 +247,53 @@ void SerialDevice::network_update()
         write_to_device(buffer, nread);
     }
 }
+
+/*
+  UDP variant of network_update().  There is no connection to accept;
+  we simply learn the autopilot's address from whichever datagram it
+  last sent us and reply to that address.  Until a first datagram has
+  arrived we have nowhere to send device->autopilot traffic, so it is
+  left unread here rather than consumed -- read_from_device() is not
+  called at all in that case, so nothing is lost; it is simply sent
+  once a peer becomes known.
+ */
+void SerialDevice::network_update_udp()
+{
+    char buffer[512];
+
+    // autopilot to device:
+    while (true) {
+        const ssize_t nread = listener->recv(buffer, sizeof(buffer), 0);
+        if (nread <= 0) {
+            break;
+        }
+        listener->last_recv_address(udp_peer_addr, udp_peer_port);
+        write_to_device(buffer, nread);
+    }
+
+    // device to autopilot:
+    if (udp_peer_addr == 0) {
+        // haven't heard from the autopilot yet, nowhere to send
+        return;
+    }
+    while (true) {
+        // AP_Networking_port::run() (AP_Networking_port.cpp) reads at
+        // most 300 bytes per recv() call.  UDP is datagram-based, not
+        // a byte stream like TCP, so a datagram larger than that isn't
+        // queued for a later read -- the kernel silently discards
+        // whatever didn't fit.  Cap what we send to what the far end
+        // can actually receive in one call, or a device that bursts
+        // more than 300 bytes at once would have its frame truncated.
+        const ssize_t nread = read_from_device(buffer, MIN(sizeof(buffer), (size_t)300));
+        if (nread <= 0) {
+            break;
+        }
+        if (listener->sendto(buffer, nread, udp_peer_addr, udp_peer_port) != nread) {
+            // if the autopilot is not keeping up we simply drop the data
+            break;
+        }
+    }
+}
 #endif  // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
 
 /**

libraries/SITL/SIM_SerialDevice.h

@@ -48,10 +48,18 @@ class SerialDevice {
 #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
     // attach this device to a TCP server socket rather than to a
     // simulated serial port.  This simulates a device which the
-    // autopilot reaches over the network (e.g. via a NET_Pn port)
-    // rather than over one of its serial ports.  Returns true on success
+    // autopilot reaches over the network (e.g. via a NET_Pn port
+    // configured as a TCP client) rather than over one of its serial
+    // ports.  Returns true on success
     bool listen_on_tcp_port(uint16_t port) WARN_IF_UNUSED;
 
+    // attach this device to a UDP socket rather than to a simulated
+    // serial port.  This simulates a device which the autopilot
+    // reaches over the network (e.g. via a NET_Pn port configured as
+    // a UDP client) rather than over one of its serial ports.
+    // Returns true on success
+    bool listen_on_udp_port(uint16_t port) WARN_IF_UNUSED;
+
     // true if this device is attached to the autopilot via a network
     // socket rather than via a simulated serial port
     bool is_network_attached() const { return listener != nullptr; }
@@ -76,13 +84,21 @@ class SerialDevice {
 
     bool is_match_baud(void) const;
 
+#if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
+    // UDP variant of network_update()
+    void network_update_udp();
+#endif
+
     // baudrate the autopilot has this device open at; zero if the
     // device is not attached to a simulated serial port
     uint32_t autopilot_baud;
 
 #if AP_SIM_SERIALDEVICE_NETWORK_ENABLED
     SocketAPM_native *listener = nullptr;  // socket the autopilot connects to, nullptr if serially attached
-    SocketAPM_native *sock = nullptr;      // socket to the connected autopilot, nullptr if not connected
+    SocketAPM_native *sock = nullptr;      // TCP: socket to the connected autopilot, nullptr if not connected.  unused for UDP
+    bool listener_is_udp;                  // true if listener is a UDP socket rather than a TCP listening socket
+    uint32_t udp_peer_addr;                // UDP: IP address of the autopilot, learned from the last packet received.  0 if no packet received yet
+    uint16_t udp_peer_port;                // UDP: port of the autopilot, learned from the last packet received
 #endif  // AP_SIM_SERIALDEVICE_NETWORK_ENABLED
 
     ssize_t corrupt_transfer(char *buffer, const ssize_t ret, const size_t size) const;