AP_HAL_Linux: RCInput_UDP: accept up to 16 channels

This allows more channels to be passed to RCInput_UDP protocol while
also allowing less than 8 channels - this is similar to the approach
used by SITL in which the packet size is used to figure out the number
of channels.

libraries/AP_HAL_Linux/RCInput_UDP.cpp

@@ -1,3 +1,5 @@
+#include <inttypes.h>
+#include <stddef.h>
 #include <stdio.h>
 
 #include <AP_HAL/AP_HAL.h>
@@ -28,26 +30,54 @@ void RCInput_UDP::init()
 
 void RCInput_UDP::_timer_tick(void)
 {
-    uint64_t delay;
-    uint16_t seq_inc;
-
-    /* Read from udp */
-    while (_socket.recv(&_buf, sizeof(_buf), 10) == sizeof(_buf)) {
-        if (_buf.version != RCINPUT_UDP_VERSION) {
-            hal.console->printf("bad protocol version for UDP RCInput\n");
-            return;
-        }
-        if (_last_buf_ts != 0 &&
-            (delay = _buf.timestamp_us - _last_buf_ts) > 100000) {
-            hal.console->printf("no rc cmds received for %llu\n", (unsigned long long)delay);
-        }
-        _last_buf_ts = _buf.timestamp_us;
+    ssize_t size = -1;
+    unsigned int n_channels;
+    ssize_t header_size = offsetof(struct rc_udp_packet, pwms);
+    uint64_t time_delta;
+    uint16_t seq_delta;
 
-        if ((seq_inc = _buf.sequence - _last_buf_seq) > 10) {
-            hal.console->printf("gap in rc cmds : %u\n", seq_inc);
+    /*
+     * Read all pending packets from udp without blocking - we may have more
+     * than one if we are not reading them faster than the RC is sending: only
+     * the last one is going to really take effect
+     */
+    do {
+        ssize_t r = _socket.recv(&_buf, sizeof(_buf), 0);
+        if (r == -1) {
+            break;
         }
-        _last_buf_seq = _buf.sequence;
 
-        _update_periods(_buf.pwms, RCINPUT_UDP_NUM_CHANNELS);
+        size = r;
+    } while (true);
+
+    if (size < header_size) {
+        return;
     }
+
+    /*
+     * v2 and v3 are compatible, with the only difference being the number
+     * of channels.  We require at least 4 channels to be compatible with
+     * the upper layers and don't care about making enforcing 8 channels
+     * for v2
+     */
+    n_channels = (size - header_size) / sizeof(uint16_t);
+    if (n_channels < 4) {
+        return;
+    }
+
+    time_delta = _buf.timestamp_us - _last_buf_ts;
+    if (_last_buf_ts != 0 && time_delta > 100000) {
+        hal.console->printf("no rc cmds received for %.2f msec\n",
+                            time_delta / 1000.0);
+    }
+
+    seq_delta = _buf.sequence - _last_buf_seq;
+    if (seq_delta > 10) {
+        hal.console->printf("gap in rc cmds > 10: %u\n", seq_delta);
+    }
+
+    _last_buf_ts = _buf.timestamp_us;
+    _last_buf_seq = _buf.sequence;
+
+    _update_periods(_buf.pwms, n_channels);
 }

libraries/AP_HAL_Linux/RCInput_UDP.h

@@ -17,7 +17,7 @@ public:
 private:
     SocketAPM   _socket{true};
     uint16_t     _port;
-    struct rc_udp_packet _buf;
+    struct rc_udp_packet_v3 _buf;
     uint64_t _last_buf_ts;
     uint16_t _last_buf_seq;
 };

libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #define RCINPUT_UDP_NUM_CHANNELS 8
-#define RCINPUT_UDP_VERSION 2
+#define RCINPUT_UDP_NUM_CHANNELS_V3 16
 
 struct __attribute__((packed)) rc_udp_packet {
     uint32_t version;
@@ -9,3 +9,11 @@ struct __attribute__((packed)) rc_udp_packet {
     uint16_t sequence;
     uint16_t pwms[RCINPUT_UDP_NUM_CHANNELS];
 };
+
+struct __attribute__((packed)) rc_udp_packet_v3 {
+    uint32_t version;
+    uint64_t timestamp_us;
+    uint16_t sequence;
+    /* Up-to 16 channels, but client can transmit less channels */
+    uint16_t pwms[RCINPUT_UDP_NUM_CHANNELS_V3];
+};