diff --git a/libraries/AP_HAL_Linux/RCInput_UDP.cpp b/libraries/AP_HAL_Linux/RCInput_UDP.cpp index 6d9fbd444e..9258a9e488 100644 --- a/libraries/AP_HAL_Linux/RCInput_UDP.cpp +++ b/libraries/AP_HAL_Linux/RCInput_UDP.cpp @@ -1,3 +1,5 @@ +#include +#include #include #include @@ -28,26 +30,54 @@ void RCInput_UDP::init() void RCInput_UDP::_timer_tick(void) { - uint64_t delay; - uint16_t seq_inc; + 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; - /* 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; + /* + * 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; } - 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; - if ((seq_inc = _buf.sequence - _last_buf_seq) > 10) { - hal.console->printf("gap in rc cmds : %u\n", seq_inc); - } - _last_buf_seq = _buf.sequence; + size = r; + } while (true); - _update_periods(_buf.pwms, RCINPUT_UDP_NUM_CHANNELS); + 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); } diff --git a/libraries/AP_HAL_Linux/RCInput_UDP.h b/libraries/AP_HAL_Linux/RCInput_UDP.h index e4979417e7..2154a26a65 100644 --- a/libraries/AP_HAL_Linux/RCInput_UDP.h +++ b/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; }; diff --git a/libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h b/libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h index 1c1c7b3c50..9537ceca41 100644 --- a/libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h +++ b/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]; +};