Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCInput_UDP: accept up to 16 channels

Browse Source 
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.

(cherry picked from commit 29d8586ea4)
This commit is contained in:
Lucas De Marchi 2020-01-15 04:22:35 -08:00 committed by Lucas De Marchi
parent 036ad45012
commit 8a01a8cddb
3 changed files with 57 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
libraries/AP_HAL_Linux/RCInput_UDP.cpp
View File

@ -1,3 +1,5 @@
#include <inttypes.h>
#include <stddef.h>
#include <stdio.h> #include <stdio.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
@ -28,26 +30,54 @@ void RCInput_UDP::init()
void RCInput_UDP::_timer_tick(void) void RCInput_UDP::_timer_tick(void)
{ {
uint64_t delay; ssize_t size = -1;
uint16_t seq_inc; 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)) { * Read all pending packets from udp without blocking - we may have more
if (_buf.version != RCINPUT_UDP_VERSION) { * than one if we are not reading them faster than the RC is sending: only
hal.console->printf("bad protocol version for UDP RCInput\n"); * the last one is going to really take effect
return; */
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) { size = r;
hal.console->printf("gap in rc cmds : %u\n", seq_inc); } while (true);
}
_last_buf_seq = _buf.sequence;
_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);
} }

2
libraries/AP_HAL_Linux/RCInput_UDP.h
View File

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

10
libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h
View File

@ -1,7 +1,7 @@
#pragma once #pragma once
#define RCINPUT_UDP_NUM_CHANNELS 8 #define RCINPUT_UDP_NUM_CHANNELS 8
#define RCINPUT_UDP_VERSION 2 #define RCINPUT_UDP_NUM_CHANNELS_V3 16
struct __attribute__((packed)) rc_udp_packet { struct __attribute__((packed)) rc_udp_packet {
uint32_t version; uint32_t version;
@ -9,3 +9,11 @@ struct __attribute__((packed)) rc_udp_packet {
uint16_t sequence; uint16_t sequence;
uint16_t pwms[RCINPUT_UDP_NUM_CHANNELS]; 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];
};