Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Lucas De Marchi 2020-01-15 04:22:35 -08:00 committed by Lucas De Marchi
parent 944939fde3
commit 29d8586ea4
3 changed files with 57 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AP_HAL_Linux/RCInput_UDP.cpp
View File

@ -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;
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");
/*
* 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;
}
size = r;
} while (true);
if (size < header_size) {
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;
if ((seq_inc = _buf.sequence - _last_buf_seq) > 10) {
hal.console->printf("gap in rc cmds : %u\n", seq_inc);
/*
* 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, RCINPUT_UDP_NUM_CHANNELS);
}
_update_periods(_buf.pwms, n_channels);
}

2
libraries/AP_HAL_Linux/RCInput_UDP.h
View File

@ -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;
};

10
libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h
View File

@ -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];
};