diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
index c2e1de6705..0a24083171 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
@@ -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)
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
index ef1b8d2536..d794f1bf3e 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -30,6 +30,8 @@ public:
SBUS_NI,
DSM,
SUMD,
+ SRXL,
+ ST24,
NONE //last enum always is None
};
void init();
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
index 23883d44b1..22cda1119e 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
@@ -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<.
+ */
+/*
+ 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< 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*/
+ 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;
+}
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h
new file mode 100644
index 0000000000..530d062628
--- /dev/null
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h
@@ -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 .
+ *
+ * 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;
+};
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp
new file mode 100644
index 0000000000..3ae2c68c48
--- /dev/null
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp
@@ -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
+ */
+#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< 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;
+ }
+}
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h
new file mode 100644
index 0000000000..82aeca26f1
--- /dev/null
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h
@@ -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 .
+ *
+ * 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; ///= SUMD_FRAME_MAXLEN) {
+ goto reset;
+ }
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
index 0e73c65cee..5c09294da6 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h
@@ -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;
};
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD_NI.h b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD_NI.h
deleted file mode 100644
index 1fe0ff5312..0000000000
--- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD_NI.h
+++ /dev/null
@@ -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 .
- *
- */
-
-#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;
-};