mirror of https://github.com/ArduPilot/ardupilot
86 lines
2.0 KiB
C++
86 lines
2.0 KiB
C++
#include <inttypes.h>
|
|
#include <stddef.h>
|
|
#include <stdio.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include "RCInput_UDP.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
using namespace Linux;
|
|
|
|
RCInput_UDP::RCInput_UDP() :
|
|
_port(0),
|
|
_last_buf_ts(0),
|
|
_last_buf_seq(0)
|
|
{}
|
|
|
|
void RCInput_UDP::init()
|
|
{
|
|
_port = RCINPUT_UDP_DEF_PORT;
|
|
if(!_socket.bind("0.0.0.0", _port)) {
|
|
hal.console->printf("failed to bind UDP socket\n");
|
|
}
|
|
|
|
_socket.set_blocking(false);
|
|
|
|
return;
|
|
}
|
|
|
|
void RCInput_UDP::_timer_tick(void)
|
|
{
|
|
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 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;
|
|
}
|
|
|
|
/*
|
|
* 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;
|
|
|
|
uint16_t pwms[n_channels];
|
|
memcpy(pwms, _buf.pwms, n_channels*sizeof(uint16_t));
|
|
_update_periods(pwms, n_channels);
|
|
}
|