diff --git a/libraries/AP_HAL_Linux/RCInput_SoloLink.cpp b/libraries/AP_HAL_Linux/RCInput_SoloLink.cpp new file mode 100644 index 0000000000..bab0f27218 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_SoloLink.cpp @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2017 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "RCInput_SoloLink.h" + +#include +#include +#include + +#include + +#define DEBUG 0 +#if DEBUG +#define debug(fmt, args...) ::printf(fmt "\n", ##args) +#else +#define debug(fmt, args...) +#endif + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +RCInput_SoloLink::RCInput_SoloLink() +{ + memset(&_packet, 0, sizeof(_packet)); +} + +void RCInput_SoloLink::init() +{ + if (!_socket.bind("0.0.0.0", PORT)) { + AP_HAL::panic("failed to bind UDP socket"); + } + + // timeout is handled by poll() in SocketAPM + _socket.set_blocking(true); + + return; +} + +bool RCInput_SoloLink::_check_hdr(ssize_t len) +{ + if (len < (ssize_t) sizeof(_packet)) { + hal.console->printf("RCInput: Packet too small (%zd), doesn't contain full frame\n", + len); + return false; + } + + uint64_t now_usec = AP_HAL::micros64(); + uint64_t delay = now_usec - _last_usec; + + if (_last_usec != 0 && delay > 40000) { + debug("RCInput: no rc cmds received for %llu\n", (unsigned long long)delay); + } + _last_usec = now_usec; + + uint16_t seq = le16toh(_packet.seq); + if (seq - _last_seq > 1) { + debug("RCInput: gap in rc cmds : %u\n", seq - _last_seq); + } + _last_seq = seq; + + return true; +} + +/* TODO: this should be a PollerThread or at least stop using a SchedThread */ +void RCInput_SoloLink::_timer_tick(void) +{ + do { + uint16_t channels[8]; + ssize_t r; + + r = _socket.recv(&_packet.buf, sizeof(_packet), 20); + if (r < 0) { + break; + } + + if (!_check_hdr(r)) { + break; + } + + channels[0] = le16toh(_packet.channel[1]); + channels[1] = le16toh(_packet.channel[2]); + channels[2] = le16toh(_packet.channel[0]); + + for (unsigned int i = 3; i < 8; i++) { + channels[i] = le16toh(_packet.channel[i]); + } + + + + _update_periods(channels, 8); + } while (true); +} diff --git a/libraries/AP_HAL_Linux/RCInput_SoloLink.h b/libraries/AP_HAL_Linux/RCInput_SoloLink.h new file mode 100644 index 0000000000..25fd5d243c --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_SoloLink.h @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2017 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include + +#include +#include + +#include "RCInput.h" + +namespace Linux { + +class RCInput_SoloLink : public RCInput +{ +public: + RCInput_SoloLink(); + + void init(); + void _timer_tick(); + +private: + static const unsigned int PACKET_LEN = 26; + static const unsigned int PORT = 5005; + + union packet { + struct PACKED { + /* changes at every packet */ + uint32_t _unknown0; + /* apparently doesn't change: version? */ + uint32_t _unknown1; + le16_t seq; + le16_t channel[8]; + }; + uint8_t buf[PACKET_LEN]; + }; + + bool _check_hdr(ssize_t len); + + SocketAPM _socket{true}; + uint64_t _last_usec = 0; + uint16_t _last_seq = 0; + union packet _packet; +}; + +}