zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

HAL_Linux: removed use of RCInput_SBUS and RCInput_115200

master
Andrew Tridgell 6 years ago
parent
1c322d296f
commit
48aacb9462
5 changed files with 2 additions and 455 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
  2. 164
      libraries/AP_HAL_Linux/RCInput_115200.cpp
  3. 55
      libraries/AP_HAL_Linux/RCInput_115200.h
  4. 180
      libraries/AP_HAL_Linux/RCInput_SBUS.cpp
  5. 52
      libraries/AP_HAL_Linux/RCInput_SBUS.h

6
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
Unescape View File

@ -24,11 +24,9 @@ @@ -24,11 +24,9 @@
#include "RCInput_Navio2.h"
#include "RCInput_PRU.h"
#include "RCInput_RPI.h"
#include "RCInput_SBUS.h"
#include "RCInput_SoloLink.h"
#include "RCInput_UART.h"
#include "RCInput_UDP.h"
#include "RCInput_115200.h"
#include "RCInput_Multi.h"
#include "RCInput_ZYNQ.h"
#include "RCInput_RCProtocol.h"
@ -138,7 +136,7 @@ static RCInput_PRU rcinDriver; @@ -138,7 +136,7 @@ static RCInput_PRU rcinDriver;
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
static RCInput_AioPRU rcinDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
static RCInput_Multi rcinDriver{2, new RCInput_AioPRU, new RCInput_115200("/dev/ttyO4")};
static RCInput_Multi rcinDriver{2, new RCInput_AioPRU, new RCInput_RCProtocol(NULL, "/dev/ttyO4")};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
@ -158,7 +156,7 @@ static RCInput_SoloLink rcinDriver; @@ -158,7 +156,7 @@ static RCInput_SoloLink rcinDriver;
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
static RCInput_Navio2 rcinDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
static RCInput_SBUS rcinDriver;
static RCInput_RCProtocol rcinDriver{"/dev/ttyPS0", NULL};
#else
static RCInput rcinDriver;
#endif

164
libraries/AP_HAL_Linux/RCInput_115200.cpp
Unescape View File

@ -1,164 +0,0 @@ @@ -1,164 +0,0 @@
/*
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/>.
*/
/*
this is a driver for R/C input protocols that use a 115200 baud
non-inverted 8-bit protocol. That includes:
- DSM
- SUMD
- ST24
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
#include "RCInput_115200.h"
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <termios.h>
extern const AP_HAL::HAL& hal;
using namespace Linux;
void RCInput_115200::init()
{
fd = open(device_path, O_RDWR | O_NONBLOCK | O_CLOEXEC);
if (fd != -1) {
struct termios options;
tcgetattr(fd, &options);
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
options.c_cflag &= ~(PARENB|CSTOPB|CSIZE);
options.c_cflag |= CS8;
options.c_lflag &= ~(ICANON|ECHO|ECHOE|ISIG);
options.c_iflag &= ~(IXON|IXOFF|IXANY);
options.c_oflag &= ~OPOST;
if (tcsetattr(fd, TCSANOW, &options) != 0) {
AP_HAL::panic("RCInput_UART: error configuring device: %s",
strerror(errno));
}
tcflush(fd, TCIOFLUSH);
}
}
void RCInput_115200::set_device_path(const char *path)
{
device_path = path;
}
void RCInput_115200::_timer_tick(void)
{
if (fd == -1) {
return;
}
// read up to 256 bytes at a time
uint8_t bytes[256];
int nread;
fd_set fds;
struct timeval tv;
FD_ZERO(&fds);
FD_SET(fd, &fds);
tv.tv_sec = 0;
tv.tv_usec = 0;
// check if any bytes are available
if (select(fd+1, &fds, nullptr, nullptr, &tv) != 1) {
return;
}
// cope with there being a large number of pending bytes at
// the start and discard them
do {
nread = ::read(fd, bytes, sizeof(bytes));
} while (nread == sizeof(bytes));
if (nread <= 0) {
return;
}
bool got_frame = false;
if (decoder == DECODER_SYNC ||
decoder == DECODER_SRXL) {
// try srxl first as it has a 16 bit CRC
if (add_srxl_input(bytes, nread)) {
// lock immediately
decoder = DECODER_SRXL;
got_frame = true;
}
}
if (decoder == DECODER_SYNC ||
decoder == DECODER_SUMD) {
// SUMD also has a 16 bit CRC
if (add_sumd_input(bytes, nread)) {
// lock immediately
decoder = DECODER_SUMD;
got_frame = true;
}
}
if (decoder == DECODER_SYNC ||
decoder == DECODER_DSM) {
// process as DSM
if (add_dsm_input(bytes, nread)) {
dsm_count++;
if (dsm_count == 10) {
// we're confident
decoder = DECODER_DSM;
}
got_frame = true;
}
}
if (decoder == DECODER_SYNC ||
decoder == DECODER_ST24) {
// process as st24
if (add_st24_input(bytes, nread)) {
st24_count++;
if (st24_count == 10) {
// we're confident
decoder = DECODER_ST24;
}
got_frame = true;
}
}
uint32_t now = AP_HAL::millis();
if (got_frame) {
last_input_ms = now;
} else if (now - last_input_ms > 1000 && decoder != DECODER_SYNC) {
// start search again
decoder = DECODER_SYNC;
dsm_count = 0;
st24_count = 0;
}
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

55
libraries/AP_HAL_Linux/RCInput_115200.h
Unescape View File

@ -1,55 +0,0 @@ @@ -1,55 +0,0 @@
/*
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/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
#include "RCInput.h"
namespace Linux {
class RCInput_115200 : public RCInput
{
public:
RCInput_115200(const char *device) :
device_path(device) {}
void init() override;
void _timer_tick(void) override;
void set_device_path(const char *path);
private:
const char *device_path;
int32_t fd = -1;
enum Decoders {
DECODER_DSM=0,
DECODER_ST24,
DECODER_SUMD,
DECODER_SRXL,
DECODER_SYNC
};
enum Decoders decoder = DECODER_SYNC;
uint8_t dsm_count;
uint8_t st24_count;
uint32_t last_input_ms;
};
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

180
libraries/AP_HAL_Linux/RCInput_SBUS.cpp
Unescape View File

@ -1,180 +0,0 @@ @@ -1,180 +0,0 @@
/*
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/>.
*/
/*
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
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
#include "RCInput_SBUS.h"
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <asm/termbits.h>
extern const AP_HAL::HAL& hal;
using namespace Linux;
#define SBUS_FRAME_SIZE 25
void RCInput_SBUS::init()
{
fd = open(device_path, O_RDWR | O_NONBLOCK | O_CLOEXEC);
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_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
// use BOTHER to specify speed directly in c_[io]speed member
tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);
tio.c_ispeed = 100000;
tio.c_ospeed = 100000;
// 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;
}
}
}
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
void RCInput_SBUS::_timer_tick(void)
{
if (fd == -1) {
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, nullptr, nullptr, &tv) != 1) {
return;
}
#if SBUS_DEBUG_LOG
static int logfd = -1;
if (logfd == -1) {
logfd = open("sbus.log", O_WRONLY|O_CREAT|O_TRUNC|O_CLOEXEC, 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;
}
}
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");
}
#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);
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

52
libraries/AP_HAL_Linux/RCInput_SBUS.h
Unescape View File

@ -1,52 +0,0 @@ @@ -1,52 +0,0 @@
/*
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/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
#include "RCInput.h"
namespace Linux {
class RCInput_SBUS : public RCInput
{
public:
void init() override;
void _timer_tick(void) override;
void set_device_path(const char *path);
private:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
const char *device_path = "/dev/ttyS1";
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
const char *device_path = "/dev/uart-sbus";
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ
const char *device_path = "/dev/ttyS2";
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ
const char *device_path = "/dev/ttyPS0";
#else
const char *device_path = nullptr;
#endif
int32_t fd = -1;
};
}
#endif // CONFIG_HAL_BOARD_SUBTYPE
Write Preview
Loading…
Cancel
Save