ardupilot/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp

348 lines
13 KiB
C++

/*
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 "AP_RCProtocol_SRXL.h"
// #define SUMD_DEBUG
extern const AP_HAL::HAL& hal;
/**
* This function calculates the 16bit crc as used throughout the srxl protocol variants
*
* This function is intended to be called whenever a new byte shall be added to the crc.
* Simply provide the old crc and the new data byte and the function return the new crc value.
*
* To start a new crc calculation for a new srxl frame, provide parameter crc=0 and the first byte of the frame.
*
* @param[in] crc - start value for crc
* @param[in] new_byte - byte that shall be included in crc calculation
* @return calculated crc
*/
uint16_t AP_RCProtocol_SRXL::srxl_crc16(uint16_t crc, uint8_t new_byte)
{
uint8_t loop;
crc = crc ^ (uint16_t)new_byte << 8;
for (loop = 0; loop < 8; loop++) {
crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1);
}
return crc;
}
void AP_RCProtocol_SRXL::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
// convert to bit widths, allowing for up to about 4usec error, assuming 115200 bps
uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000;
uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000;
uint8_t bit_ofs, byte_ofs;
uint16_t nbits;
if (bits_s0 == 0 || bits_s1 == 0) {
// invalid data
goto reset;
}
byte_ofs = srxl_state.bit_ofs/10;
bit_ofs = srxl_state.bit_ofs%10;
if (byte_ofs >= SRXL_FRAMELEN_MAX) {
goto reset;
}
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {
nbits = 10 - bit_ofs;
}
srxl_state.bytes[byte_ofs] |= ((1U<<nbits)-1) << bit_ofs;
srxl_state.bit_ofs += nbits;
bit_ofs += nbits;
if (bits_s0 - nbits > 10) {
// we have a full frame
uint8_t byte;
uint8_t i;
for (i=0; i <= byte_ofs; i++) {
// get raw data
uint16_t v = srxl_state.bytes[i];
// check start bit
if ((v & 1) != 0) {
break;
}
// check stop bits
if ((v & 0x200) != 0x200) {
break;
}
byte = ((v>>1) & 0xFF);
process_byte(byte);
}
memset(&srxl_state, 0, sizeof(srxl_state));
}
byte_ofs = srxl_state.bit_ofs/10;
bit_ofs = srxl_state.bit_ofs%10;
if (bits_s1+bit_ofs > 10) {
// invalid data
goto reset;
}
// pull in the low bits
srxl_state.bit_ofs += bits_s1;
return;
reset:
memset(&srxl_state, 0, sizeof(srxl_state));
}
/**
* 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
*/
int AP_RCProtocol_SRXL::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
*/
int AP_RCProtocol_SRXL::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;
}
} 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;
}
void AP_RCProtocol_SRXL::process_byte(uint8_t byte)
{
uint64_t timestamp_us = AP_HAL::micros64();
/*----------------------------------------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; /* 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 = srxl_crc16(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;
}
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 = srxl_crc16(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 */
uint16_t values[SRXL_MAX_CHANNELS];
uint8_t num_values;
bool failsafe_state;
switch (frame_header) {
case SRXL_HEADER_V1:
srxl_channels_get_v1v2(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state);
add_input(num_values, values, failsafe_state);
break;
case SRXL_HEADER_V2:
srxl_channels_get_v1v2(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state);
add_input(num_values, values, failsafe_state);
break;
case SRXL_HEADER_V5:
srxl_channels_get_v5(MAX_RCIN_CHANNELS, &num_values, values, &failsafe_state);
add_input(num_values, values, failsafe_state);
break;
default:
break;
}
}
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:
break;
} /* switch (decode_state) */
decode_state = decode_state_next;
last_data_us = timestamp_us;
}