mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 14:13:42 -04:00
79b004cf6a
This allows to use a Solo controller to control Linux-based flight controllers. The protocol has been derived by analyzing a tcpdump trace: some fields are ignored. Example trace of RC data (obtained with `tshark -T fields -e data -n -c 5 -r rc.pcap` unkonwn seq ch1 ch2 ch3 ... ch8 5fa8 f441 3414 0500 73d7 dc05 dc05 dc05 db05 e803 e803 e803 f401 73f6 f441 3414 0500 74d7 dc05 dc05 dc05 db05 e803 e803 e803 f401 dc44 f541 3414 0500 75d7 dc05 dc05 dc05 db05 e803 e803 e803 f401 bc92 f541 3414 0500 76d7 dc05 dc05 dc05 db05 e803 e803 e803 f401 dfe0 f541 3414 0500 77d7 dc05 dc05 dc05 db05 e803 e803 e803 f401
107 lines
2.6 KiB
C++
107 lines
2.6 KiB
C++
/*
|
|
* 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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
#include "RCInput_SoloLink.h"
|
|
|
|
#include <errno.h>
|
|
#include <inttypes.h>
|
|
#include <stdio.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#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);
|
|
}
|