diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index b8d8114357..f390c68315 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -26,6 +26,7 @@ namespace Linux { class LinuxRCInput_AioPRU; class LinuxRCInput_Navio; class LinuxRCInput_ZYNQ; + class LinuxRCInput_UDP; class LinuxRCOutput_PRU; class LinuxRCOutput_AioPRU; class LinuxRCOutput_PCA9685; diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index 10c2da9c80..5c9dca73ef 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -17,6 +17,7 @@ #include "RCInput.h" #include "RCInput_AioPRU.h" #include "RCInput_Navio.h" +#include "RCInput_UDP.h" #include "RCOutput_PRU.h" #include "RCOutput_AioPRU.h" #include "RCOutput_PCA9685.h" diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 2f8f8fba63..996d929033 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -79,6 +79,8 @@ static LinuxRCInput_AioPRU rcinDriver; static LinuxRCInput_Navio rcinDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ static LinuxRCInput_ZYNQ rcinDriver; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP +static LinuxRCInput_UDP rcinDriver; #else static LinuxRCInput rcinDriver; #endif diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 722c461efd..68c46154cb 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -337,4 +337,19 @@ void LinuxRCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1) _process_dsm_pulse(width_s0, width_s1); } +/* + * Update channel values directly + */ +void LinuxRCInput::_update_periods(uint16_t *periods, uint8_t len) +{ + if (len > LINUX_RC_INPUT_NUM_CHANNELS) { + len = LINUX_RC_INPUT_NUM_CHANNELS; + } + for (unsigned int i=0; i < len; i++) { + _pwm_values[i] = periods[i]; + } + _num_channels = len; + new_rc_input = true; +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index a938de2eee..ce2a78a395 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -25,6 +25,7 @@ public: protected: void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1); + void _update_periods(uint16_t *periods, uint8_t len); private: volatile bool new_rc_input; diff --git a/libraries/AP_HAL_Linux/RCInput_UDP.cpp b/libraries/AP_HAL_Linux/RCInput_UDP.cpp new file mode 100644 index 0000000000..b6c75e2efe --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_UDP.cpp @@ -0,0 +1,54 @@ +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP +#include "RCInput_UDP.h" +#include + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +LinuxRCInput_UDP::LinuxRCInput_UDP() : + _port(0), + _last_buf_ts(0), + _last_buf_seq(0) +{} + +void LinuxRCInput_UDP::init(void *) +{ + _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 LinuxRCInput_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", 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); + } +} +#endif diff --git a/libraries/AP_HAL_Linux/RCInput_UDP.h b/libraries/AP_HAL_Linux/RCInput_UDP.h new file mode 100644 index 0000000000..4ee6de6986 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_UDP.h @@ -0,0 +1,24 @@ + +#ifndef _AP_HAL_LINUX_RCINPUT_UDP_H +#define _AP_HAL_LINUX_RCINPUT_UDP_H + +#include "RCInput.h" +#include +#include "RCInput_UDP_Protocol.h" + +#define RCINPUT_UDP_DEF_PORT 777 + +class Linux::LinuxRCInput_UDP : public Linux::LinuxRCInput +{ +public: + LinuxRCInput_UDP(); + void init(void*); + void _timer_tick(void); +private: + SocketAPM _socket{true}; + uint16_t _port; + struct rc_udp_packet _buf; + uint64_t _last_buf_ts; + uint16_t _last_buf_seq; +}; +#endif // _AP_HAL_LINUX_RCINPUT_UDP_H diff --git a/libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h b/libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h new file mode 100644 index 0000000000..0f26968679 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_UDP_Protocol.h @@ -0,0 +1,14 @@ +#ifndef _RCINPUT_UDP_PROTOCOL_H +#define _RCINPUT_UDP_PROTOCOL_H + +#define RCINPUT_UDP_NUM_CHANNELS 8 +#define RCINPUT_UDP_VERSION 2 + +struct __attribute__((packed)) rc_udp_packet { + uint32_t version; + uint64_t timestamp_us; + uint16_t sequence; + uint16_t pwms[RCINPUT_UDP_NUM_CHANNELS]; +}; + +#endif