You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
53 lines
1.2 KiB
53 lines
1.2 KiB
#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) |
|
{ |
|
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; |
|
|
|
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; |
|
|
|
_update_periods(_buf.pwms, RCINPUT_UDP_NUM_CHANNELS); |
|
} |
|
}
|
|
|