AP_HAL: added an SRXL protocol decoder
SRXL ports are commonly found on newer spektrum receivers like the AR7700
This commit is contained in:
Andrew Tridgell
parent
bb1a787852
commit
7b3d0234d1
217
libraries/AP_HAL/utility/srxl.cpp
Normal file
217
libraries/AP_HAL/utility/srxl.cpp
Normal file
@ -0,0 +1,217 @@
|
||||
// -*- tab-width: 8; Mode: C++; c-basic-offset: 8; indent-tabs-mode: -*- t -*-
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
SRXL protocol decoder, tested against AR7700 SRXL port
|
||||
Andrew Tridgell, September 2016
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include "srxl.h"
|
||||
|
||||
/*
|
||||
there are other SRXL varients, but we need some sample data before accepting them
|
||||
*/
|
||||
#define SRXL_MAX_CHANNELS 16
|
||||
#define SRXL_HEAD_MARKER 0xA5
|
||||
|
||||
#define SRXL_NUM_BYTES 18
|
||||
|
||||
static uint8_t buffer[SRXL_NUM_BYTES];
|
||||
static uint8_t buflen;
|
||||
static uint64_t last_data_us;
|
||||
static uint16_t channels[SRXL_MAX_CHANNELS];
|
||||
static uint16_t max_channels;
|
||||
|
||||
/*
|
||||
get SRXL crc16 for a set of bytes
|
||||
*/
|
||||
static uint16_t srxl_crc16(const uint8_t *bytes, uint8_t nbytes)
|
||||
{
|
||||
uint16_t crc = 0;
|
||||
while (nbytes--) {
|
||||
int i;
|
||||
crc ^= (uint16_t)(*bytes++) << 8;
|
||||
|
||||
for (i = 0; i < 8; i++) {
|
||||
crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1);
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
decode a SRXL packet
|
||||
*/
|
||||
int srxl_decode(uint64_t timestamp_us, uint8_t byte, uint8_t *num_values, uint16_t *values, uint16_t max_values)
|
||||
{
|
||||
if (timestamp_us - last_data_us > 5000U || buflen == SRXL_NUM_BYTES) {
|
||||
// we've seen a frame gap
|
||||
if (buflen != 0) {
|
||||
buflen = 0;
|
||||
}
|
||||
}
|
||||
last_data_us = timestamp_us;
|
||||
buffer[buflen++] = byte;
|
||||
|
||||
switch (buffer[0]) {
|
||||
case SRXL_HEAD_MARKER:
|
||||
break;
|
||||
|
||||
default:
|
||||
// invalid packet, we don't support this format yet
|
||||
return 2;
|
||||
}
|
||||
|
||||
uint8_t expected_bytes = SRXL_NUM_BYTES;
|
||||
if (buflen < expected_bytes) {
|
||||
// more bytes pending
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint16_t crc = srxl_crc16(buffer, buflen-2);
|
||||
uint64_t crc2 = buffer[buflen-1] | (buffer[buflen-2]<<8);
|
||||
if (crc != crc2) {
|
||||
// bad CRC
|
||||
return 4;
|
||||
}
|
||||
|
||||
// up to 7 channel values per packet. Each channel value is 16
|
||||
// bits, with 11 bits of data and 4 bits of channel number. The
|
||||
// top bit indicates a special X-Plus channel
|
||||
for (uint8_t i=0; i<7; i++) {
|
||||
uint16_t b = buffer[i*2+2] << 8 | buffer[i*2+3];
|
||||
uint16_t c = b >> 11; // channel number
|
||||
int32_t v = b & 0x7FF;
|
||||
|
||||
// if channel number if greater than 16 then it is a X-Plus
|
||||
// channel. We don't yet know how to decode those. There is some information here:
|
||||
// http://www.deviationtx.com/forum/protocol-development/2088-18-channels-for-dsm2-dsmx?start=40
|
||||
// but we really need some sample data to confirm
|
||||
if (c < SRXL_MAX_CHANNELS) {
|
||||
v = (((v - 0x400) * 500) / 876) + 1500;
|
||||
channels[c] = v;
|
||||
if (c >= max_channels) {
|
||||
max_channels = c+1;
|
||||
}
|
||||
}
|
||||
//printf("%u:%u ", (unsigned)c, (unsigned)v);
|
||||
}
|
||||
//printf("\n");
|
||||
|
||||
*num_values = max_channels;
|
||||
if (*num_values > max_values) {
|
||||
*num_values = max_values;
|
||||
}
|
||||
memcpy(values, channels, (*num_values)*2);
|
||||
|
||||
#if 0
|
||||
for (uint8_t i=0; i<buflen; i++) {
|
||||
printf("%02x ", (unsigned)buffer[i]);
|
||||
}
|
||||
printf("\n");
|
||||
#endif
|
||||
|
||||
buflen = 0;
|
||||
|
||||
// success
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef TEST_MAIN_PROGRAM
|
||||
/*
|
||||
test harness for use under Linux with USB serial adapter
|
||||
*/
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <termios.h>
|
||||
|
||||
static uint64_t micros64(void)
|
||||
{
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)));
|
||||
}
|
||||
|
||||
int main(int argc, const char *argv[])
|
||||
{
|
||||
int fd = open(argv[1], O_RDONLY);
|
||||
if (fd == -1) {
|
||||
perror(argv[1]);
|
||||
exit(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) {
|
||||
perror("tcsetattr");
|
||||
exit(1);
|
||||
}
|
||||
tcflush(fd, TCIOFLUSH);
|
||||
|
||||
while (true) {
|
||||
uint8_t b;
|
||||
uint8_t num_values = 0;
|
||||
uint16_t values[18];
|
||||
fd_set fds;
|
||||
struct timeval tv;
|
||||
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(fd, &fds);
|
||||
|
||||
tv.tv_sec = 1;
|
||||
tv.tv_usec = 0;
|
||||
|
||||
// check if any bytes are available
|
||||
if (select(fd+1, &fds, NULL, NULL, &tv) != 1) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (read(fd, &b, 1) != 1) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (srxl_decode(micros64(), b, &num_values, values, 18) == 0) {
|
||||
#if 1
|
||||
printf("%u: ", num_values);
|
||||
for (uint8_t i=0; i<num_values; i++) {
|
||||
printf("%u:%4u ", i+1, values[i]);
|
||||
}
|
||||
printf("\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
28
libraries/AP_HAL/utility/srxl.h
Normal file
28
libraries/AP_HAL/utility/srxl.h
Normal file
@ -0,0 +1,28 @@
|
||||
// -*- tab-width: 8; Mode: C++; c-basic-offset: 8; indent-tabs-mode: -*- t -*-
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
SRXL protocol decoder
|
||||
Andrew Tridgell, September 2016
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/*
|
||||
* Decoder for SRXL protocol
|
||||
*
|
||||
* @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 4 for checksum error
|
||||
*/
|
||||
int srxl_decode(uint64_t timestamp_us, uint8_t byte, uint8_t *num_values, uint16_t *values, uint16_t max_values);
|
||||
Loading…
Reference in New Issue
Block a user