ardupilot/libraries/AP_HAL_Linux/RCInput_SBUS.cpp

183 lines
5.3 KiB
C++
Raw Normal View History

2016-06-07 02:06:35 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program 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 program 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/>.
*/
/*
2016-07-13 01:26:54 -03:00
this is a driver for SBUS input in Linux board using a UART. Note
that it relies on kernel support for 100kbaud and on a uart inverted
in hardware
2016-06-07 02:06:35 -03:00
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#include "RCInput_SBUS.h"
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <asm/termbits.h>
2016-06-07 02:06:35 -03:00
extern const AP_HAL::HAL& hal;
using namespace Linux;
#ifndef B100000
// disco uses 100000 for baud rate to give 100000 baud
#define B100000 100000
#endif
#define SBUS_FRAME_SIZE 25
2016-06-07 02:06:35 -03:00
void RCInput_SBUS::init()
{
fd = open(device_path, O_RDWR | O_NONBLOCK);
if (fd != -1) {
printf("Opened SBUS input %s fd=%d\n", device_path, (int)fd);
fflush(stdout);
struct termios2 tio {};
if (ioctl(fd, TCGETS2, &tio) != 0) {
close(fd);
fd = -1;
return;
}
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR
| IGNCR | ICRNL | IXON);
tio.c_iflag |= (INPCK | IGNPAR);
tio.c_oflag &= ~OPOST;
tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
tio.c_ispeed = B100000;
tio.c_ospeed = B100000;
tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
// see select() comment below
tio.c_cc[VMIN] = SBUS_FRAME_SIZE;
tio.c_cc[VTIME] = 0;
if (ioctl(fd, TCSETS2, &tio) != 0) {
close(fd);
fd = -1;
return;
}
}
2016-06-07 02:06:35 -03:00
}
void RCInput_SBUS::set_device_path(const char *path)
{
device_path = path;
printf("Set SBUS device path %s\n", path);
}
#define SBUS_DEBUG_LOG 0
#define SBUS_CAUSE_CORRUPTION 0
2016-06-07 02:06:35 -03:00
void RCInput_SBUS::_timer_tick(void)
{
if (fd == -1) {
2016-06-07 02:06:35 -03:00
return;
}
// read up to 10 frames at a time
uint8_t bytes[SBUS_FRAME_SIZE*10];
int nread;
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(fd, &fds);
tv.tv_sec = 0;
tv.tv_usec = 0;
// as VMIN is SBUS_FRAME_SIZE the select won't return unless there is
// at least SBUS_FRAME_SIZE bytes available
if (select(fd+1, &fds, NULL, NULL, &tv) != 1) {
return;
}
#if SBUS_DEBUG_LOG
static int logfd = -1;
if (logfd == -1) {
logfd = open("sbus.log", O_WRONLY|O_CREAT|O_TRUNC, 0644);
}
#endif
#if SBUS_CAUSE_CORRUPTION
// deliberately lose bytes from the port
static unsigned corruption_counter;
if (corruption_counter++ % 1000 == 0) {
uint8_t nn = corruption_counter/1000;
int n2 = ::read(fd, bytes, nn);
dprintf(logfd, "throw %u\n", (unsigned)n2);
}
#endif
// cope with there being a large number of pending bytes at
// the start
do {
nread = ::read(fd, bytes, sizeof(bytes));
} while (nread == sizeof(bytes));
if (nread % SBUS_FRAME_SIZE != 0) {
/*
SBUS frames are 25 bytes long, and always start with
0x0f, but there is no other framing information to
prevent us getting out of sync. All we have are the
timing guarantees
In this case we have read a partial frame, or lost some
bytes. A 25 bytes frame at 100000 baud takes 2.5ms. By
delaying 3.5ms here we will get any remaining bytes for
this frame. We shouldn't get the start of the next frame
as frames happen at most every 7ms
This strategy allows us to resync even if we lose
bytes. It assumes an interframe gap of more than
3.5ms. If SBUS is run at very high rate (like 300Hz)
then this won't work
*/
hal.scheduler->delay_microseconds(3500);
int n2 = ::read(fd, bytes+nread, sizeof(bytes)-nread);
if (n2 > 0) {
nread += n2;
2016-06-07 02:06:35 -03:00
}
}
if (nread % SBUS_FRAME_SIZE != 0) {
// if we don't have a multuple of 25 then throw away the lot
#if SBUS_DEBUG_LOG
dprintf(logfd, "discard %u\n", (unsigned)nread);
#endif
return;
}
if (nread <= 0) {
return;
}
#if SBUS_DEBUG_LOG
if (logfd != -1) {
dprintf(logfd, "%06u %u: ", (unsigned)AP_HAL::millis(), (unsigned)nread);
for (uint8_t i=0; i<nread; i++) {
dprintf(logfd, "%02x ", (unsigned)bytes[i]);
}
dprintf(logfd, "\n");
2016-06-07 02:06:35 -03:00
}
#endif
// only process the last SBUS_FRAME_SIZE bytes. Only the latest
// frame matters
add_sbus_input(&bytes[nread-SBUS_FRAME_SIZE], SBUS_FRAME_SIZE);
2016-06-07 02:06:35 -03:00
}
#endif // CONFIG_HAL_BOARD_SUBTYPE