Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: add support for srxl and st24 protocol

This commit is contained in:
Siddharth Purohit 2018-07-13 20:19:42 +05:30 committed by Andrew Tridgell
parent f99bd32b51
commit baff7291ce
10 changed files with 865 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_RCProtocol/AP_RCProtocol.cpp
View File

@ -21,7 +21,8 @@
#include "AP_RCProtocol_SBUS.h"
#include "AP_RCProtocol_SBUS_NI.h"
#include "AP_RCProtocol_SUMD.h"
#include "AP_RCProtocol_SUMD_NI.h"
#include "AP_RCProtocol_SRXL.h"
#include "AP_RCProtocol_ST24.h"
void AP_RCProtocol::init()
{
@ -30,6 +31,8 @@ void AP_RCProtocol::init()
backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS_NI(*this);
backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this);
backend[AP_RCProtocol::SUMD] = new AP_RCProtocol_SUMD(*this);
backend[AP_RCProtocol::SRXL] = new AP_RCProtocol_SRXL(*this);
backend[AP_RCProtocol::ST24] = new AP_RCProtocol_ST24(*this);
}
void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)

2
libraries/AP_RCProtocol/AP_RCProtocol.h
View File

@ -30,6 +30,8 @@ public:
SBUS_NI,
DSM,
SUMD,
SRXL,
ST24,
NONE //last enum always is None
};
void init();

3
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
View File

@ -222,6 +222,9 @@ void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1)
goto reset;
}
if (byte_ofs > 25) {
goto reset;
}
// pull in the high bits
sbus_state.bytes[byte_ofs] |= ((1U<<bits_s0)-1) << bit_ofs;
sbus_state.bit_ofs += bits_s0;

348
libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp Normal file
View File

@ -0,0 +1,348 @@
/*
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;
}

68
libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h Normal file
View File

@ -0,0 +1,68 @@
/*
* This file 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 file 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/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#pragma once
#include "AP_RCProtocol.h"
#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 */
/* 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*/
class AP_RCProtocol_SRXL : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_SRXL(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte) override;
private:
static uint16_t srxl_crc16(uint16_t crc, uint8_t new_byte);
int srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state);
int srxl_channels_get_v5(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state);
uint8_t buffer[SRXL_FRAMELEN_MAX]; /* buffer for raw srxl frame data in correct order --> buffer[0]=byte0 buffer[1]=byte1 */
uint8_t buflen; /* length in number of bytes of received srxl dataframe in buffer */
uint64_t last_data_us; /* timespan since last received data in us */
uint16_t channels[SRXL_MAX_CHANNELS] = {0}; /* buffer for extracted RC channel data as pulsewidth in microseconds */
uint16_t max_channels = 0;
enum {
STATE_IDLE, /* do nothing */
STATE_NEW, /* get header of frame + prepare for frame reception + begin new crc cycle */
STATE_COLLECT /* collect RC channel data from frame + concurrently calc crc over payload data + extract channel information */
};
uint8_t frame_header = 0U; /* Frame header from SRXL datastream */
uint8_t frame_len_full = 0U; /* Length in number of bytes of full srxl datastream */
uint8_t decode_state = STATE_IDLE; /* Current state of SRXL frame decoding */
uint8_t decode_state_next = STATE_IDLE; /* State of frame decoding thatwill be applied when the next byte from dataframe drops in */
uint16_t crc_fmu = 0U; /* CRC calculated over payload from srxl datastream on this machine */
uint16_t crc_receiver = 0U; /* CRC extracted from srxl datastream */
struct {
uint16_t bytes[SRXL_FRAMELEN_MAX];
uint16_t bit_ofs;
} srxl_state;
};

282
libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp Normal file
View File

@ -0,0 +1,282 @@
/*
SUMD decoder, based on PX4Firmware/src/rc/lib/rc/sumd.c from PX4Firmware
modified for use in AP_HAL_* by Andrew Tridgell
*/
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file sumd.h
*
* RC protocol definition for Graupner HoTT transmitter (SUMD/SUMH Protocol)
*
* @author Marco Bauer <marco@wtns.de>
*/
#include "AP_RCProtocol_ST24.h"
// #define SUMD_DEBUG
extern const AP_HAL::HAL& hal;
uint8_t AP_RCProtocol_ST24::st24_crc8(uint8_t *ptr, uint8_t len)
{
uint8_t i, crc ;
crc = 0;
while (len--) {
for (i = 0x80; i != 0; i >>= 1) {
if ((crc & 0x80) != 0) {
crc <<= 1;
crc ^= 0x07;
} else {
crc <<= 1;
}
if ((*ptr & i) != 0) {
crc ^= 0x07;
}
}
ptr++;
}
return (crc);
}
void AP_RCProtocol_ST24::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 = st24_state.bit_ofs/10;
bit_ofs = st24_state.bit_ofs%10;
if (byte_ofs >= ST24_MAX_FRAMELEN) {
goto reset;
}
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {
nbits = 10 - bit_ofs;
}
st24_state.bytes[byte_ofs] |= ((1U<<nbits)-1) << bit_ofs;
st24_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 = st24_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(&st24_state, 0, sizeof(st24_state));
}
byte_ofs = st24_state.bit_ofs/10;
bit_ofs = st24_state.bit_ofs%10;
if (bits_s1+bit_ofs > 10) {
// invalid data
goto reset;
}
// pull in the low bits
st24_state.bit_ofs += bits_s1;
return;
reset:
memset(&st24_state, 0, sizeof(st24_state));
}
void AP_RCProtocol_ST24::process_byte(uint8_t byte)
{
switch (_decode_state) {
case ST24_DECODE_STATE_UNSYNCED:
if (byte == ST24_STX1) {
_decode_state = ST24_DECODE_STATE_GOT_STX1;
}
break;
case ST24_DECODE_STATE_GOT_STX1:
if (byte == ST24_STX2) {
_decode_state = ST24_DECODE_STATE_GOT_STX2;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
case ST24_DECODE_STATE_GOT_STX2:
/* ensure no data overflow failure or hack is possible */
if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
_rxpacket.length = byte;
_rxlen = 0;
_decode_state = ST24_DECODE_STATE_GOT_LEN;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
case ST24_DECODE_STATE_GOT_LEN:
_rxpacket.type = byte;
_rxlen++;
_decode_state = ST24_DECODE_STATE_GOT_TYPE;
break;
case ST24_DECODE_STATE_GOT_TYPE:
_rxpacket.st24_data[_rxlen - 1] = byte;
_rxlen++;
if (_rxlen == (_rxpacket.length - 1)) {
_decode_state = ST24_DECODE_STATE_GOT_DATA;
}
break;
case ST24_DECODE_STATE_GOT_DATA:
_rxpacket.crc8 = byte;
_rxlen++;
if (st24_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
/* decode the actual packet */
switch (_rxpacket.type) {
case ST24_PACKET_TYPE_CHANNELDATA12: {
uint16_t values[12];
uint8_t num_values;
ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
//TBD: add support for RSSI
// *rssi = d->rssi;
//*rx_count = d->packet_count;
/* this can lead to rounding of the strides */
num_values = (MAX_RCIN_CHANNELS < 12) ? MAX_RCIN_CHANNELS : 12;
unsigned stride_count = (num_values * 3) / 2;
unsigned chan_index = 0;
for (unsigned i = 0; i < stride_count; i += 3) {
values[chan_index] = ((uint16_t)d->channel[i] << 4);
values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
values[chan_index] = ((uint16_t)d->channel[i + 2]);
values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
}
}
break;
case ST24_PACKET_TYPE_CHANNELDATA24: {
uint16_t values[24];
uint8_t num_values;
ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
//*rssi = d->rssi;
//*rx_count = d->packet_count;
/* this can lead to rounding of the strides */
num_values = (MAX_RCIN_CHANNELS < 24) ? MAX_RCIN_CHANNELS : 24;
unsigned stride_count = (num_values * 3) / 2;
unsigned chan_index = 0;
for (unsigned i = 0; i < stride_count; i += 3) {
values[chan_index] = ((uint16_t)d->channel[i] << 4);
values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
values[chan_index] = ((uint16_t)d->channel[i + 2]);
values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
}
}
break;
case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
// ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
/* we silently ignore this data for now, as it is unused */
}
break;
default:
break;
}
} else {
/* decoding failed */
}
_decode_state = ST24_DECODE_STATE_UNSYNCED;
break;
}
}

153
libraries/AP_RCProtocol/AP_RCProtocol_ST24.h Normal file
View File

@ -0,0 +1,153 @@
/*
* This file 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 file 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/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#pragma once
#include "AP_RCProtocol.h"
#define ST24_DATA_LEN_MAX 64
#define ST24_MAX_FRAMELEN 70
#define ST24_STX1 0x55
#define ST24_STX2 0x55
/* define range mapping here, -+100% -> 1000..2000 */
#define ST24_RANGE_MIN 0.0f
#define ST24_RANGE_MAX 4096.0f
#define ST24_TARGET_MIN 1000.0f
#define ST24_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
class AP_RCProtocol_ST24 : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_ST24(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte) override;
private:
static uint8_t st24_crc8(uint8_t *ptr, uint8_t len);
enum ST24_PACKET_TYPE {
ST24_PACKET_TYPE_CHANNELDATA12 = 0,
ST24_PACKET_TYPE_CHANNELDATA24,
ST24_PACKET_TYPE_TRANSMITTERGPSDATA
};
#pragma pack(push, 1)
typedef struct {
uint8_t header1; ///< 0x55 for a valid packet
uint8_t header2; ///< 0x55 for a valid packet
uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
uint8_t type; ///< from enum ST24_PACKET_TYPE
uint8_t st24_data[ST24_DATA_LEN_MAX];
uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
} ReceiverFcPacket;
/**
* RC Channel data (12 channels).
*
* This is incoming from the ST24
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers)
} ChannelData12;
/**
* RC Channel data (12 channels).
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers)
} ChannelData24;
/**
* Telemetry packet
*
* This is outgoing to the ST24
*
* imuStatus:
* 8 bit total
* bits 0-2 for status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - values 3 through 7 are reserved
* bits 3-7 are status for sensors (0 or 1)
* - mpu6050
* - accelerometer
* - primary gyro x
* - primary gyro y
* - primary gyro z
*
* pressCompassStatus
* 8 bit total
* bits 0-3 for compass status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - value 3 - 15 are reserved
* bits 4-7 for pressure status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - value 3 - 15 are reserved
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
int32_t lat; ///< lattitude (degrees) +/- 90 deg
int32_t lon; ///< longitude (degrees) +/- 180 deg
int32_t alt; ///< 0.01m resolution, altitude (meters)
int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down
uint8_t nsat; ///<number of satellites
uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V
uint8_t current; ///< 0.5A resolution
int16_t roll, pitch, yaw; ///< 0.01 degree resolution
uint8_t motorStatus; ///< 1 bit per motor for status 1=good, 0= fail
uint8_t imuStatus; ///< inertial measurement unit status
uint8_t pressCompassStatus; ///< baro / compass status
} TelemetryData;
#pragma pack(pop)
enum ST24_DECODE_STATE {
ST24_DECODE_STATE_UNSYNCED = 0,
ST24_DECODE_STATE_GOT_STX1,
ST24_DECODE_STATE_GOT_STX2,
ST24_DECODE_STATE_GOT_LEN,
ST24_DECODE_STATE_GOT_TYPE,
ST24_DECODE_STATE_GOT_DATA
};
enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
uint8_t _rxlen;
ReceiverFcPacket _rxpacket;
struct {
uint16_t bytes[70];
uint16_t bit_ofs;
bool packet_parsed;
} st24_state;
};

4
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp
View File

@ -97,7 +97,9 @@ void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1)
byte_ofs = sumd_state.bit_ofs/10;
bit_ofs = sumd_state.bit_ofs%10;
if (byte_ofs >= SUMD_FRAME_MAXLEN) {
goto reset;
}
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {

4
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
View File

@ -19,7 +19,7 @@
#include "AP_RCProtocol.h"
#define SUMD_MAX_CHANNELS 32
#define SUMD_FRAME_MAXLEN 40
class AP_RCProtocol_SUMD : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_SUMD(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
@ -63,7 +63,7 @@ private:
bool _crcOK = false;
struct {
uint16_t bytes[40];
uint8_t bit_ofs;
uint16_t bit_ofs;
bool packet_parsed;
} sumd_state;
};

31
libraries/AP_RCProtocol/AP_RCProtocol_SUMD_NI.h
View File

@ -1,31 +0,0 @@
/*
* This file 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 file 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_RCProtocol.h"
#include "AP_RCProtocol_SUMD.h"
class AP_RCProtocol_SUMD_NI : public AP_RCProtocol_SUMD {
public:
AP_RCProtocol_SUMD_NI(AP_RCProtocol &_frontend) : AP_RCProtocol_SUMD(_frontend), saved_width(0) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override {
AP_RCProtocol_SUMD::process_pulse(saved_width, width_s0);
saved_width = width_s1;
}
private:
uint32_t saved_width;
};