Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 14:13:42 -04:00
Code Issues Packages Projects Releases Wiki Activity
3a72cd1ce2
ardupilot/libraries/AP_HAL/utility/srxl.cpp
Peter Barker 10675ab5c4 AP_HAL: correct include ordering in srxl.h 
The header needs stdint.h which it was only getting because it was
included after stdint.h in the cpp file.

Stop including standard headers before other ArduPilot headers
2019-09-11 14:19:14 +10:00

492 lines
18 KiB
C++
Raw Blame History

/*
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
Co author: Roman Kirchner, September 2016
- 2016.10.23: SRXL variant V1 sucessfully (Testbench and Pixhawk/MissionPlanner) tested with RX-9-DR M-LINK (SW v1.26)
*/
#include "srxl.h"
#include <AP_Math/crc.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
/* SRXL datastream characteristics for all variants */
#define SRXL_MIN_FRAMESPACE_US 8000U /* Minumum space between srxl frames in us (applies to all variants) */
#define SRXL_MAX_CHANNELS 20U /* Maximum number of channels from srxl datastream */
/* decode progress states */
#define STATE_IDLE 0x00U /* do nothing */
#define STATE_NEW 0x01U /* get header of frame + prepare for frame reception + begin new crc cycle */
#define STATE_COLLECT 0x02U /* collect RC channel data from frame + concurrently calc crc over payload data + extract channel information */
/* Variant specific SRXL datastream characteristics */
/* Framelength in byte */
#define SRXL_FRAMELEN_V1 27U /* Framelength with header in byte for: Mpx SRXLv1 or XBUS Mode B */
#define SRXL_FRAMELEN_V2 35U /* Framelength with header in byte for: Mpx SRXLv2 */
#define SRXL_FRAMELEN_V5 18U /* Framelength with header in byte for Spk AR7700 etc. */
#define SRXL_FRAMELEN_MAX 35U /* maximum possible framelengh */
/* Headerbyte */
#define SRXL_HEADER_V1 0xA1U /* Headerbyte for: Mpx SRXLv1 or XBUS Mode B */
#define SRXL_HEADER_V2 0xA2U /* Headerbyte for: Mpx SRXLv2 */
#define SRXL_HEADER_V5 0xA5U /* Headerbyte for: Spk AR7700 etc. */
#define SRXL_HEADER_NOT_IMPL 0xFFU /* Headerbyte for non impemented srxl header*/
static uint8_t buffer[SRXL_FRAMELEN_MAX]; /* buffer for raw srxl frame data in correct order --> buffer[0]=byte0 buffer[1]=byte1 */
static uint8_t buflen; /* length in number of bytes of received srxl dataframe in buffer */
static uint64_t last_data_us; /* timespan since last received data in us */
static uint16_t channels[SRXL_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */
static uint8_t frame_header = 0U; /* Frame header from SRXL datastream */
static uint8_t frame_len_full = 0U; /* Length in number of bytes of full srxl datastream */
static uint8_t decode_state = STATE_IDLE; /* Current state of SRXL frame decoding */
static uint8_t decode_state_next = STATE_IDLE; /* State of frame decoding thatwill be applied when the next byte from dataframe drops in */
static uint16_t crc_fmu = 0U; /* CRC calculated over payload from srxl datastream on this machine */
static uint16_t crc_receiver = 0U; /* CRC extracted from srxl datastream */
static uint16_t max_channels;
/**
* Get RC channel information as microsecond pulsewidth representation from srxl version 1 and 2
*
* This function extracts RC channel information from srxl dataframe. The function expects the whole dataframe
* in correct order in static array "buffer[SRXL_FRAMELEN_MAX]". After extracting all RC channel information, the data
* is transferred to "values" array from parameter list. If the pixhawk does not support all channels from srxl datastream,
* only supported number of channels will be refreshed.
*
* IMPORTANT SAFETY NOTICE: This function shall only be used after CRC has been successful.
*
* Structure of SRXL v1 dataframe --> 12 channels, 12 Bit per channel
* Byte0: Header 0xA1
* Byte1: Bits7-4:Empty Bits3-0:Channel1 MSB
* Byte2: Bits7-0: Channel1 LSB
* (....)
* Byte23: Bits7-4:Empty Bits3-0:Channel12 MSB
* Byte24: Bits7-0: Channel12 LSB
* Byte25: CRC16 MSB
* Byte26: CRC16 LSB
*
* Structure of SRXL v2 dataframe --> 16 channels, 12 Bit per channel
* Byte0: Header 0xA2
* Byte1: Bits7-4:Empty Bits3-0:Channel1 MSB
* Byte2: Bits7-0: Channel1 LSB
* (....)
* Byte31: Bits7-4:Empty Bits3-0:Channel16 MSB
* Byte32: Bits7-0: Channel16 LSB
* Byte33: CRC16 MSB
* Byte34: CRC16 LSB
*
* @param[in] max_values - maximum number of values supported by the pixhawk
* @param[out] num_values - number of RC channels extracted from srxl frame
* @param[out] values - array of RC channels with refreshed information as pulsewidth in microseconds Range: 800us - 2200us
* @param[out] failsafe_state - true: RC-receiver is in failsafe state, false: RC-receiver is not in failsafe state
* @retval 0 success
*/
static int srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state)
{
uint8_t loop;
uint32_t channel_raw_value;
*num_values = (uint8_t)((frame_len_full - 3U)/2U);
*failsafe_state = 0U; /* this protocol version does not support failsafe information */
/* get data channel data from frame */
for (loop=0U; loop < *num_values; loop++) {
channel_raw_value = ((((uint32_t)buffer[loop*2U+1U])& 0x0000000FU) << 8U) | ((uint32_t)(buffer[loop*2U+2U])); /* get 12bit channel raw value from srxl datastream (mask out unused bits with 0x0000000F) */
channels[loop] = (uint16_t)(((channel_raw_value * (uint32_t)1400U) >> 12U) + (uint32_t)800U); /* convert raw value to servo/esc signal pulsewidth in us */
}
/* provide channel data to FMU */
if ( (uint16_t)*num_values > max_values) {
*num_values = (uint8_t)max_values;
}
memcpy(values, channels, (*num_values)*2);
return 0; /* for srxl protocol version 1 and 2 it is not expected, that any error happen during decode process */
}
/**
* Get RC channel information as microsecond pulsewidth representation from srxl version 5
*
* This function extracts RC channel information from srxl dataframe. The function expects the whole dataframe
* in correct order in static array "buffer[SRXL_FRAMELEN_MAX]". After extracting all RC channel information, the data
* is transferred to "values" array from parameter list. If the pixhawk does not support all channels from srxl datastream,
* only supported number of channels will be refreshed.
*
* IMPORTANT SAFETY NOTICE: This function shall only be used after CRC has been successful.
*
* Structure of SRXL v5 dataframe
* Byte0: Header 0xA5
* Byte1 - Byte16: Payload
* Byte17: CRC16 MSB
* Byte18: CRC16 LSB
*
* @param[in] max_values - maximum number of values supported by the pixhawk
* @param[out] num_values - number of RC channels extracted from srxl frame
* @param[out] values - array of RC channels with refreshed information as pulsewidth in microseconds Range: 800us - 2200us
* @param[out] failsafe_state - true: RC-receiver is in failsafe state, false: RC-receiver is not in failsafe state
* @retval 0 success
*/
static int srxl_channels_get_v5(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state)
{
// 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 (b & 0x8000) {
continue;
}
if (c == 12) {
// special handling for channel 12
// see http://www.deviationtx.com/forum/protocol-development/2088-18-channels-for-dsm2-dsmx?start=40
//printf("c12: 0x%x %02x %02x\n", (unsigned)(b>>9), (unsigned)buffer[0], (unsigned)buffer[1]);
v = (b & 0x1FF) << 2;
c = 10 + ((b >> 9) & 0x7);
if (buffer[1] & 1) {
c += 4;
}
#if 0
printf("b=0x%04x v=%u c=%u b[1]=0x%02x\n",
(unsigned)b, (unsigned)v, (unsigned)c, (unsigned)buffer[1]);
#endif
} else if (c > 12) {
// invalid
v = 0;
}
// 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);
// check failsafe bit, this goes low when connection to the
// transmitter is lost
*failsafe_state = ((buffer[1] & 2) == 0);
// success
return 0;
}
/**
* Decode SRXL frames
*
*
* Structure of all SRXL frames
* Byte[0]: Header 0xA<VARIANT> --> Variant specific header. Variant is encoded in bits 3-0 of header byte.
* Byte[1] - Byte[N-2]: SRXL variant specific payload
* Byte[N-1] - Byte[N]: CRC16 over payload and header
*
* @param[in] timestamp_us - timestamp in microseconds
* @param[in] received byte in microseconds
* @param[out] num_values - number of RC channels extracted from srxl frame
* @param[out] values - array of RC channels with refreshed information as pulsewidth in microseconds Range: 800us - 2200us
* @param[in] maximum number of values supported by pixhawk
* @param[out] failsafe_state - true: RC-receiver is in failsafe state, false: RC-receiver is not in failsafe state
* @retval 0 success (a decoded packet)
* @retval 1 no packet yet (accumulating)
* @retval 2 unknown packet
* @retval 4 checksum error
*/
int srxl_decode(uint64_t timestamp_us, uint8_t byte, uint8_t *num_values, uint16_t *values, uint16_t max_values, bool *failsafe_state)
{
int ret = 1;
/*----------------------------------------distinguish different srxl variants at the beginning of each frame---------------------------------------------- */
/* Check if we have a new begin of a frame --> indicators: Time gap in datastream + SRXL header 0xA<VARIANT>*/
if ( (timestamp_us - last_data_us) >= SRXL_MIN_FRAMESPACE_US) {
/* Now detect SRXL variant based on header */
switch(byte) {
case SRXL_HEADER_V1:
frame_len_full = SRXL_FRAMELEN_V1;
frame_header = SRXL_HEADER_V1;
decode_state = STATE_NEW;
break;
case SRXL_HEADER_V2:
frame_len_full = SRXL_FRAMELEN_V2;
frame_header = SRXL_HEADER_V2;
decode_state = STATE_NEW;
break;
case SRXL_HEADER_V5:
frame_len_full = SRXL_FRAMELEN_V5;
frame_header = SRXL_HEADER_V5;
decode_state = STATE_NEW;
break;
default:
frame_len_full = 0U;
frame_header = SRXL_HEADER_NOT_IMPL;
decode_state = STATE_IDLE;
buflen = 0;
return 2; /* protocol version not implemented --> no channel data --> unknown packet */
}
}
/*--------------------------------------------collect all data from stream and decode-------------------------------------------------------*/
switch (decode_state) {
case STATE_NEW: /* buffer header byte and prepare for frame reception and decoding */
buffer[0U]=byte;
crc_fmu = crc_xmodem_update(0U,byte);
buflen = 1U;
decode_state_next = STATE_COLLECT;
break;
case STATE_COLLECT: /* receive all bytes. After reception decode frame and provide rc channel information to FMU */
if (buflen >= frame_len_full) {
// a logic bug in the state machine, this shouldn't happen
decode_state = STATE_IDLE;
buflen = 0;
frame_len_full = 0;
frame_header = SRXL_HEADER_NOT_IMPL;
return 2;
}
buffer[buflen] = byte;
buflen++;
/* CRC not over last 2 frame bytes as these bytes inhabitate the crc */
if (buflen <= (frame_len_full-2)) {
crc_fmu = crc_xmodem_update(crc_fmu,byte);
}
if( buflen == frame_len_full ) {
/* CRC check here */
crc_receiver = ((uint16_t)buffer[buflen-2] << 8U) | ((uint16_t)buffer[buflen-1]);
if (crc_receiver == crc_fmu) {
/* at this point buffer contains all frame data and crc is valid --> extract channel info according to SRXL variant */
switch (frame_header) {
case SRXL_HEADER_V1:
ret = srxl_channels_get_v1v2(max_values, num_values, values, failsafe_state);
break;
case SRXL_HEADER_V2:
ret = srxl_channels_get_v1v2(max_values, num_values, values, failsafe_state);
break;
case SRXL_HEADER_V5:
ret = srxl_channels_get_v5(max_values, num_values, values, failsafe_state);
break;
default:
ret = 2; /* protocol version not implemented --> no channel data */
break;
}
}
else {
ret = 4; /* CRC fail --> no channel data */
}
decode_state_next = STATE_IDLE; /* frame data buffering and decoding finished --> statemachine not in use until new header drops is */
}
else {
/* frame not completely received --> frame data buffering still ongoing */
decode_state_next = STATE_COLLECT;
}
break;
default:
ret = 1; /* STATE_IDLE --> do nothing */
break;
} /* switch (decode_state) */
decode_state = decode_state_next;
last_data_us = timestamp_us;
return ret;
}
#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|O_CLOEXEC);
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[20];
bool failsafe_state;
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, sizeof(values)/sizeof(values[0]), &failsafe_state) == 0) {
#if 1
printf("%u: ", num_values);
for (uint8_t i=0; i<num_values; i++) {
printf("%u:%4u ", i+1, values[i]);
}
printf("%s\n", failsafe_state?"FAIL":"OK");
#endif
}
}
}
#endif
#ifdef TEST_BIN_PROGRAM
/*
test harness for use under Linux with hex dump in a file
*/
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <time.h>
#include <unistd.h>
#include <stdlib.h>
#include <termios.h>
int main(int argc, const char *argv[])
{
FILE *f = fopen(argv[1], "r");
if (!f) {
perror(argv[1]);
exit(1);
}
uint64_t t=0;
while (true) {
uint8_t b;
uint8_t num_values = 0;
uint16_t values[20];
bool failsafe_state;
uint8_t header;
if (fread(&header, 1, 1, f) != 1) {
break;
}
uint8_t frame_size = 0;
switch (header) {
case SRXL_HEADER_V1:
frame_size = SRXL_FRAMELEN_V1;
break;
case SRXL_HEADER_V2:
frame_size = SRXL_FRAMELEN_V2;
break;
case SRXL_HEADER_V5:
frame_size = SRXL_FRAMELEN_V5;
break;
}
if (frame_size == 0) {
continue;
}
uint8_t u[frame_size];
u[0] = header;
if (fread(&u[1], 1, sizeof(u)-1, f) != sizeof(u)-1) {
break;
}
t += 11000;
for (uint8_t i=0; i<sizeof(u); i++) {
b = u[i];
if (srxl_decode(t, b, &num_values, values, sizeof(values)/sizeof(values[0]), &failsafe_state) == 0) {
printf("%u: ", num_values);
for (uint8_t i=0; i<num_values; i++) {
printf("%u:%4u ", i+1, values[i]);
}
printf("%s\n", failsafe_state?"FAIL":"OK");
}
}
}
fclose(f);
}
#endif
Reference in New Issue View Git Blame Copy Permalink