Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: bring up to date with master

Browse Source 
this adds new RC protocol decoders for SRXL, SUMD and ST24 as well as
updating to latest soft-serial decoder
This commit is contained in:
Andrew Tridgell 2019-01-09 13:50:11 +11:00
parent 51ee86583b
commit 9467e0fa04
18 changed files with 2043 additions and 450 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
libraries/AP_RCProtocol/AP_RCProtocol.cpp
View File

@ -11,7 +11,7 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
@ -19,25 +19,116 @@
#include "AP_RCProtocol_PPMSum.h" #include "AP_RCProtocol_PPMSum.h"
#include "AP_RCProtocol_DSM.h" #include "AP_RCProtocol_DSM.h"
#include "AP_RCProtocol_SBUS.h" #include "AP_RCProtocol_SBUS.h"
#include "AP_RCProtocol_SBUS_NI.h" #include "AP_RCProtocol_SUMD.h"
#include "AP_RCProtocol_SRXL.h"
#include "AP_RCProtocol_ST24.h"
// singleton
AP_RCProtocol *AP_RCProtocol::instance;
void AP_RCProtocol::init() void AP_RCProtocol::init()
{ {
backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this); backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this);
backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this); backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this, true);
backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS_NI(*this); backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS(*this, false);
backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*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);
}
AP_RCProtocol::~AP_RCProtocol()
{
for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
if (backend[i] != nullptr) {
delete backend[i];
backend[i] = nullptr;
}
}
instance = nullptr;
} }
void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
{ {
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
bool searching = (now - _last_input_ms >= 200);
if (_detected_protocol != AP_RCProtocol::NONE && _detected_with_bytes && !searching) {
// we're using byte inputs, discard pulses
return;
}
// first try current protocol // first try current protocol
if (_detected_protocol != AP_RCProtocol::NONE && now - _last_input_ms < 200) { if (_detected_protocol != AP_RCProtocol::NONE && !searching) {
backend[_detected_protocol]->process_pulse(width_s0, width_s1); backend[_detected_protocol]->process_pulse(width_s0, width_s1);
if (backend[_detected_protocol]->new_input()) { if (backend[_detected_protocol]->new_input()) {
_new_input = true; _new_input = true;
_last_input_ms = AP_HAL::millis(); _last_input_ms = now;
}
return;
}
// otherwise scan all protocols
for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
if (_disabled_for_pulses & (1U << i)) {
// this protocol is disabled for pulse input
continue;
}
if (backend[i] != nullptr) {
uint32_t frame_count = backend[i]->get_rc_frame_count();
uint32_t input_count = backend[i]->get_rc_input_count();
backend[i]->process_pulse(width_s0, width_s1);
if (frame_count != backend[i]->get_rc_frame_count()) {
_good_frames[i]++;
if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) {
continue;
}
_new_input = (input_count != backend[i]->get_rc_input_count());
_detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;
memset(_good_frames, 0, sizeof(_good_frames));
_last_input_ms = now;
_detected_with_bytes = false;
break;
}
}
}
}
/*
process an array of pulses. n must be even
*/
void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap)
{
if (n & 1) {
return;
}
while (n) {
uint32_t widths0 = widths[0];
uint32_t widths1 = widths[1];
if (need_swap) {
uint32_t tmp = widths1;
widths1 = widths0;
widths0 = tmp;
}
widths1 -= widths0;
process_pulse(widths0, widths1);
widths += 2;
n -= 2;
}
}
void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
{
uint32_t now = AP_HAL::millis();
bool searching = (now - _last_input_ms >= 200);
if (_detected_protocol != AP_RCProtocol::NONE && !_detected_with_bytes && !searching) {
// we're using pulse inputs, discard bytes
return;
}
// first try current protocol
if (_detected_protocol != AP_RCProtocol::NONE && !searching) {
backend[_detected_protocol]->process_byte(byte, baudrate);
if (backend[_detected_protocol]->new_input()) {
_new_input = true;
_last_input_ms = now;
} }
return; return;
} }
@ -45,11 +136,20 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
// otherwise scan all protocols // otherwise scan all protocols
for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
if (backend[i] != nullptr) { if (backend[i] != nullptr) {
backend[i]->process_pulse(width_s0, width_s1); uint32_t frame_count = backend[i]->get_rc_frame_count();
if (backend[i]->new_input()) { uint32_t input_count = backend[i]->get_rc_input_count();
_new_input = true; backend[i]->process_byte(byte, baudrate);
if (frame_count != backend[i]->get_rc_frame_count()) {
_good_frames[i]++;
if (requires_3_frames((rcprotocol_t)i) && _good_frames[i] < 3) {
continue;
}
_new_input = (input_count != backend[i]->get_rc_input_count());
_detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;
_last_input_ms = AP_HAL::millis(); memset(_good_frames, 0, sizeof(_good_frames));
_last_input_ms = now;
_detected_with_bytes = true;
break;
} }
} }
} }
@ -96,3 +196,36 @@ void AP_RCProtocol::start_bind(void)
} }
} }
} }
/*
return protocol name
*/
const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
{
switch (protocol) {
case PPM:
return "PPM";
case SBUS:
case SBUS_NI:
return "SBUS";
case DSM:
return "DSM";
case SUMD:
return "SUMD";
case SRXL:
return "SRXL";
case ST24:
return "ST24";
case NONE:
break;
}
return nullptr;
}
/*
return protocol name
*/
const char *AP_RCProtocol::protocol_name(void) const
{
return protocol_name_from_protocol(_detected_protocol);
}

58
libraries/AP_RCProtocol/AP_RCProtocol.h
View File

@ -11,39 +11,89 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#define MAX_RCIN_CHANNELS 16 #define MAX_RCIN_CHANNELS 32
#define MIN_RCIN_CHANNELS 5 #define MIN_RCIN_CHANNELS 5
class AP_RCProtocol_Backend; class AP_RCProtocol_Backend;
class AP_RCProtocol { class AP_RCProtocol {
public: public:
enum rcprotocol_t{ AP_RCProtocol() {
instance = this;
}
~AP_RCProtocol();
enum rcprotocol_t {
PPM = 0, PPM = 0,
SBUS, SBUS,
SBUS_NI, SBUS_NI,
DSM, DSM,
SUMD,
SRXL,
ST24,
NONE //last enum always is None NONE //last enum always is None
}; };
void init(); void init();
bool valid_serial_prot()
{
return _valid_serial_prot;
}
void process_pulse(uint32_t width_s0, uint32_t width_s1); void process_pulse(uint32_t width_s0, uint32_t width_s1);
enum rcprotocol_t protocol_detected() { return _detected_protocol; } void process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap);
void process_byte(uint8_t byte, uint32_t baudrate);
void disable_for_pulses(enum rcprotocol_t protocol) {
_disabled_for_pulses |= (1U<<(uint8_t)protocol);
}
// for protocols without strong CRCs we require 3 good frames to lock on
bool requires_3_frames(enum rcprotocol_t p) {
return (p == DSM || p == SBUS || p == SBUS_NI || p == PPM);
}
enum rcprotocol_t protocol_detected()
{
return _detected_protocol;
}
uint8_t num_channels(); uint8_t num_channels();
uint16_t read(uint8_t chan); uint16_t read(uint8_t chan);
bool new_input(); bool new_input();
void start_bind(void); void start_bind(void);
// return protocol name as a string
static const char *protocol_name_from_protocol(rcprotocol_t protocol);
// return protocol name as a string
const char *protocol_name(void) const;
// return protocol name as a string
enum rcprotocol_t protocol_detected(void) const {
return _detected_protocol;
}
// access to singleton
static AP_RCProtocol *get_instance(void) {
return instance;
}
private: private:
enum rcprotocol_t _detected_protocol = NONE; enum rcprotocol_t _detected_protocol = NONE;
uint16_t _disabled_for_pulses;
bool _detected_with_bytes;
AP_RCProtocol_Backend *backend[NONE]; AP_RCProtocol_Backend *backend[NONE];
bool _new_input = false; bool _new_input = false;
uint32_t _last_input_ms; uint32_t _last_input_ms;
bool _valid_serial_prot = false;
uint8_t _good_frames[NONE];
static AP_RCProtocol *instance;
}; };
#include "AP_RCProtocol_Backend.h" #include "AP_RCProtocol_Backend.h"

5
libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
View File

@ -11,14 +11,14 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
#include "AP_RCProtocol.h" #include "AP_RCProtocol.h"
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) :
frontend(_frontend), frontend(_frontend),
rc_input_count(0), rc_input_count(0),
last_rc_input_count(0), last_rc_input_count(0),
@ -52,6 +52,7 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool
num_values = MIN(num_values, MAX_RCIN_CHANNELS); num_values = MIN(num_values, MAX_RCIN_CHANNELS);
memcpy(_pwm_values, values, num_values*sizeof(uint16_t)); memcpy(_pwm_values, values, num_values*sizeof(uint16_t));
_num_channels = num_values; _num_channels = num_values;
rc_frame_count++;
if (!in_failsafe) { if (!in_failsafe) {
rc_input_count++; rc_input_count++;
} }

30
libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
View File

@ -11,7 +11,7 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
@ -19,13 +19,14 @@
#include "AP_RCProtocol.h" #include "AP_RCProtocol.h"
class AP_RCProtocol_Backend class AP_RCProtocol_Backend {
{
friend class AP_RCProtcol; friend class AP_RCProtcol;
public: public:
AP_RCProtocol_Backend(AP_RCProtocol &_frontend); AP_RCProtocol_Backend(AP_RCProtocol &_frontend);
virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) = 0; virtual ~AP_RCProtocol_Backend() {}
virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) {}
virtual void process_byte(uint8_t byte, uint32_t baudrate) {}
uint16_t read(uint8_t chan); uint16_t read(uint8_t chan);
bool new_input(); bool new_input();
uint8_t num_channels(); uint8_t num_channels();
@ -35,14 +36,29 @@ public:
// allow for backends that need regular polling // allow for backends that need regular polling
virtual void update(void) {} virtual void update(void) {}
enum {
PARSE_TYPE_SIGREAD,
PARSE_TYPE_SERIAL
};
// get number of frames, ignoring failsafe
uint32_t get_rc_frame_count(void) const {
return rc_frame_count;
}
// get number of frames, honoring failsafe
uint32_t get_rc_input_count(void) const {
return rc_input_count;
}
protected: protected:
void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe); void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe);
private: private:
AP_RCProtocol &frontend; AP_RCProtocol &frontend;
unsigned int rc_input_count; uint32_t rc_input_count;
unsigned int last_rc_input_count; uint32_t last_rc_input_count;
uint32_t rc_frame_count;
uint16_t _pwm_values[MAX_RCIN_CHANNELS]; uint16_t _pwm_values[MAX_RCIN_CHANNELS];
uint8_t _num_channels; uint8_t _num_channels;

536
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -11,7 +11,7 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
/* /*
@ -21,9 +21,9 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// #define DEBUG // #define DSM_DEBUG
#ifdef DEBUG #ifdef DSM_DEBUG
# define debug(fmt, args...) hal.console->printf(fmt "\n", ##args) # define debug(fmt, args...) printf(fmt "\n", ##args)
#else #else
# define debug(fmt, args...) do {} while(0) # define debug(fmt, args...) do {} while(0)
#endif #endif
@ -34,74 +34,10 @@ extern const AP_HAL::HAL& hal;
void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1) void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
{ {
// convert to bit widths, allowing for up to about 4usec error, assuming 115200 bps uint8_t b;
uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000; if (ss.process_pulse(width_s0, width_s1, b)) {
uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000; _process_byte(ss.get_byte_timestamp_us()/1000U, b);
uint8_t bit_ofs, byte_ofs;
uint16_t nbits;
if (bits_s0 == 0 || bits_s1 == 0) {
// invalid data
goto reset;
} }
byte_ofs = dsm_state.bit_ofs/10;
bit_ofs = dsm_state.bit_ofs%10;
if(byte_ofs > 15) {
// invalid data
goto reset;
}
// pull in the high bits
nbits = bits_s0;
if (nbits+bit_ofs > 10) {
nbits = 10 - bit_ofs;
}
dsm_state.bytes[byte_ofs] |= ((1U<<nbits)-1) << bit_ofs;
dsm_state.bit_ofs += nbits;
bit_ofs += nbits;
if (bits_s0 - nbits > 10) {
if (dsm_state.bit_ofs == 16*10) {
// we have a full frame
uint8_t bytes[16];
uint8_t i;
for (i=0; i<16; i++) {
// get raw data
uint16_t v = dsm_state.bytes[i];
// check start bit
if ((v & 1) != 0) {
goto reset;
}
// check stop bits
if ((v & 0x200) != 0x200) {
goto reset;
}
bytes[i] = ((v>>1) & 0xFF);
}
if (dsm_decode(AP_HAL::micros64(), bytes, last_values, &num_channels, AP_DSM_MAX_CHANNELS) &&
num_channels >= MIN_RCIN_CHANNELS) {
add_input(num_channels, last_values, false);
}
}
memset(&dsm_state, 0, sizeof(dsm_state));
}
byte_ofs = dsm_state.bit_ofs/10;
bit_ofs = dsm_state.bit_ofs%10;
if (bits_s1+bit_ofs > 10) {
// invalid data
goto reset;
}
// pull in the low bits
dsm_state.bit_ofs += bits_s1;
return;
reset:
memset(&dsm_state, 0, sizeof(dsm_state));
} }
/** /**
@ -132,17 +68,18 @@ reset:
bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{ {
if (raw == 0xffff) if (raw == 0xffff) {
return false; return false;
}
*channel = (raw >> shift) & 0xf; *channel = (raw >> shift) & 0xf;
uint16_t data_mask = (1 << shift) - 1; uint16_t data_mask = (1 << shift) - 1;
*value = raw & data_mask; *value = raw & data_mask;
//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
return true; return true;
} }
/** /**
@ -152,212 +89,213 @@ bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigne
*/ */
void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]) void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16])
{ {
static uint32_t cs10; /* reset the 10/11 bit sniffed channel masks */
static uint32_t cs11; if (reset) {
static unsigned samples; cs10 = 0;
cs11 = 0;
samples = 0;
channel_shift = 0;
return;
}
/* reset the 10/11 bit sniffed channel masks */ /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
if (reset) { for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
cs10 = 0;
cs11 = 0;
samples = 0;
dsm_channel_shift = 0;
return;
}
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ const uint8_t *dp = &dsm_frame[2 + (2 * i)];
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
const uint8_t *dp = &dsm_frame[2 + (2 * i)]; /* if the channel decodes, remember the assigned number */
uint16_t raw = (dp[0] << 8) | dp[1]; if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) {
unsigned channel, value; cs10 |= (1 << channel);
}
/* if the channel decodes, remember the assigned number */ if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) {
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) cs11 |= (1 << channel);
cs10 |= (1 << channel); }
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
cs11 |= (1 << channel); }
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ /* wait until we have seen plenty of frames - 5 should normally be enough */
} if (samples++ < 5) {
return;
}
/* wait until we have seen plenty of frames - 5 should normally be enough */ /*
if (samples++ < 5) * Iterate the set of sensible sniffed channel sets and see whether
return; * decoding in 10 or 11-bit mode has yielded anything we recognize.
*
* XXX Note that due to what seem to be bugs in the DSM2 high-resolution
* stream, we may want to sniff for longer in some cases when we think we
* are talking to a DSM2 receiver in high-resolution mode (so that we can
* reject it, ideally).
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
static const uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
0x1ff, /* 9 channels (DX9, etc.) */
0x3ff, /* 10 channels (DX10) */
0x1fff, /* 13 channels (DX10t) */
0x3fff /* 18 channels (DX10) */
};
unsigned votes10 = 0;
unsigned votes11 = 0;
/* for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) {
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognize.
*
* XXX Note that due to what seem to be bugs in the DSM2 high-resolution
* stream, we may want to sniff for longer in some cases when we think we
* are talking to a DSM2 receiver in high-resolution mode (so that we can
* reject it, ideally).
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
static uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
0x1ff, /* 9 channels (DX9, etc.) */
0x3ff, /* 10 channels (DX10) */
0x1fff, /* 13 channels (DX10t) */
0x3fff /* 18 channels (DX10) */
};
unsigned votes10 = 0;
unsigned votes11 = 0;
for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) { if (cs10 == masks[i]) {
votes10++;
}
if (cs10 == masks[i]) if (cs11 == masks[i]) {
votes10++; votes11++;
}
}
if (cs11 == masks[i]) if ((votes11 == 1) && (votes10 == 0)) {
votes11++; channel_shift = 11;
} debug("DSM: 11-bit format");
return;
}
if ((votes11 == 1) && (votes10 == 0)) { if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 11; channel_shift = 10;
debug("DSM: 11-bit format"); debug("DSM: 10-bit format");
return; return;
} }
if ((votes10 == 1) && (votes11 == 0)) { /* call ourselves to reset our state ... we have to try again */
dsm_channel_shift = 10; debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
debug("DSM: 10-bit format"); dsm_guess_format(true, dsm_frame);
return;
}
/* call ourselves to reset our state ... we have to try again */
debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
dsm_guess_format(true, dsm_frame);
} }
/** /**
* Decode the entire dsm frame (all contained channels) * Decode the entire dsm frame (all contained channels)
* *
*/ */
bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values) uint16_t *values, uint16_t *num_values, uint16_t max_values)
{ {
#if 0 /*
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", * If we have lost signal for at least 200ms, reset the
dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], * format guessing heuristic.
dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); */
#endif if (((frame_time_ms - last_frame_time_ms) > 200U) && (channel_shift != 0)) {
/* dsm_guess_format(true, dsm_frame);
* If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) {
dsm_guess_format(true, dsm_frame);
} }
/* we have received something we think is a dsm_frame */ /* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time; last_frame_time_ms = frame_time_ms;
/* if we don't know the dsm_frame format, update the guessing state machine */ /* if we don't know the dsm_frame format, update the guessing state machine */
if (dsm_channel_shift == 0) { if (channel_shift == 0) {
dsm_guess_format(false, dsm_frame); dsm_guess_format(false, dsm_frame);
return false; return false;
} }
/* /*
* The encoding of the first two bytes is uncertain, so we're * The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now. * going to ignore them for now.
* *
* Each channel is a 16-bit unsigned value containing either a 10- * Each channel is a 16-bit unsigned value containing either a 10-
* or 11-bit channel value and a 4-bit channel number, shifted * or 11-bit channel value and a 4-bit channel number, shifted
* either 10 or 11 bits. The MSB may also be set to indicate the * either 10 or 11 bits. The MSB may also be set to indicate the
* second dsm_frame in variants of the protocol where more than * second dsm_frame in variants of the protocol where more than
* seven channels are being transmitted. * seven channels are being transmitted.
*/ */
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
const uint8_t *dp = &dsm_frame[2 + (2 * i)]; const uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1]; uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value; unsigned channel, value;
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) {
continue; continue;
}
/* ignore channels out of range */ /* ignore channels out of range */
if (channel >= max_values) if (channel >= max_values) {
continue; continue;
}
/* update the decoded channel count */ /* update the decoded channel count */
if (channel >= *num_values) if (channel >= *num_values) {
*num_values = channel + 1; *num_values = channel + 1;
}
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
if (dsm_channel_shift == 10) if (channel_shift == 10) {
value *= 2; value *= 2;
}
/* /*
* Spektrum scaling is special. There are these basic considerations * Spektrum scaling is special. There are these basic considerations
* *
* * Midpoint is 1520 us * * Midpoint is 1520 us
* * 100% travel channels are +- 400 us * * 100% travel channels are +- 400 us
* *
* We obey the original Spektrum scaling (so a default setup will scale from * We obey the original Spektrum scaling (so a default setup will scale from
* 1100 - 1900 us), but we do not obey the weird 1520 us center point * 1100 - 1900 us), but we do not obey the weird 1520 us center point
* and instead (correctly) center the center around 1500 us. This is in order * and instead (correctly) center the center around 1500 us. This is in order
* to get something useful without requiring the user to calibrate on a digital * to get something useful without requiring the user to calibrate on a digital
* link for no reason. * link for no reason.
*/ */
/* scaled integer for decent accuracy while staying efficient */ /* scaled integer for decent accuracy while staying efficient */
value = ((((int)value - 1024) * 1000) / 1700) + 1500; value = ((((int)value - 1024) * 1000) / 1700) + 1500;
/* /*
* Store the decoded channel into the R/C input buffer, taking into * Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have. * account the different ideas about channel assignement that we have.
* *
* Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
* but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
*/ */
switch (channel) { switch (channel) {
case 0: case 0:
channel = 2; channel = 2;
break; break;
case 1: case 1:
channel = 0; channel = 0;
break; break;
case 2: case 2:
channel = 1; channel = 1;
default: default:
break; break;
} }
values[channel] = value; values[channel] = value;
} }
/* /*
* Spektrum likes to send junk in higher channel numbers to fill * Spektrum likes to send junk in higher channel numbers to fill
* their packets. We don't know about a 13 channel model in their TX * their packets. We don't know about a 13 channel model in their TX
* lines, so if we get a channel count of 13, we'll return 12 (the last * lines, so if we get a channel count of 13, we'll return 12 (the last
* data index that is stable). * data index that is stable).
*/ */
if (*num_values == 13) if (*num_values == 13) {
*num_values = 12; *num_values = 12;
}
#if 0 #if 0
if (dsm_channel_shift == 11) { if (channel_shift == 11) {
/* Set the 11-bit data indicator */ /* Set the 11-bit data indicator */
*num_values |= 0x8000; *num_values |= 0x8000;
} }
#endif #endif
/* /*
* XXX Note that we may be in failsafe here; we need to work out how to detect that. * XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/ */
return true; return true;
} }
@ -408,14 +346,14 @@ void AP_RCProtocol_DSM::update(void)
hal.scheduler->delay_microseconds(120); hal.scheduler->delay_microseconds(120);
hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 0); hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 0);
hal.scheduler->delay_microseconds(120); hal.scheduler->delay_microseconds(120);
hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1); hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1);
} }
bind_last_ms = now; bind_last_ms = now;
bind_state = BIND_STATE4; bind_state = BIND_STATE4;
} }
break; break;
} }
case BIND_STATE4: { case BIND_STATE4: {
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - bind_last_ms > 50) { if (now - bind_last_ms > 50) {
@ -427,3 +365,115 @@ void AP_RCProtocol_DSM::update(void)
} }
#endif #endif
} }
/*
parse one DSM byte, maintaining decoder state
*/
bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values,
uint16_t *num_values, uint16_t max_channels)
{
/* this is set by the decoding state machine and will default to false
* once everything that was decodable has been decoded.
*/
bool decode_ret = false;
/* overflow check */
if (byte_input.ofs == sizeof(byte_input.buf) / sizeof(byte_input.buf[0])) {
byte_input.ofs = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
debug("DSM: RESET (BUF LIM)\n");
}
if (byte_input.ofs == DSM_FRAME_SIZE) {
byte_input.ofs = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
debug("DSM: RESET (PACKET LIM)\n");
}
#ifdef DSM_DEBUG
debug("dsm state: %s%s, count: %d, val: %02x\n",
(dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
(dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
byte_input.ofs,
(unsigned)b);
#endif
switch (dsm_decode_state) {
case DSM_DECODE_STATE_DESYNC:
/* we are de-synced and only interested in the frame marker */
if ((frame_time_ms - last_rx_time_ms) >= 5) {
dsm_decode_state = DSM_DECODE_STATE_SYNC;
byte_input.ofs = 0;
chan_count = 0;
byte_input.buf[byte_input.ofs++] = b;
}
break;
case DSM_DECODE_STATE_SYNC: {
if ((frame_time_ms - last_rx_time_ms) >= 5 && byte_input.ofs > 0) {
byte_input.ofs = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
break;
}
byte_input.buf[byte_input.ofs++] = b;
/* decode whatever we got and expect */
if (byte_input.ofs < DSM_FRAME_SIZE) {
break;
}
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
decode_ret = dsm_decode(frame_time_ms, byte_input.buf, values, &chan_count, max_channels);
/* we consumed the partial frame, reset */
byte_input.ofs = 0;
/* if decoding failed, set proto to desync */
if (decode_ret == false) {
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
}
break;
}
default:
debug("UNKNOWN PROTO STATE");
decode_ret = false;
}
if (decode_ret) {
*num_values = chan_count;
}
last_rx_time_ms = frame_time_ms;
/* return false as default */
return decode_ret;
}
// support byte input
void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_ms, uint8_t b)
{
uint16_t v[AP_DSM_MAX_CHANNELS];
uint16_t nchan;
memcpy(v, last_values, sizeof(v));
if (dsm_parse_byte(timestamp_ms, b, v, &nchan, AP_DSM_MAX_CHANNELS)) {
memcpy(last_values, v, sizeof(v));
if (nchan >= MIN_RCIN_CHANNELS) {
add_input(nchan, last_values, false);
}
}
}
// support byte input
void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(AP_HAL::millis(), b);
}

42
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h
View File

@ -11,13 +11,14 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
#pragma once #pragma once
#include "AP_RCProtocol.h" #include "AP_RCProtocol.h"
#include "SoftSerial.h"
#define AP_DSM_MAX_CHANNELS 12 #define AP_DSM_MAX_CHANNELS 12
@ -25,23 +26,27 @@ class AP_RCProtocol_DSM : public AP_RCProtocol_Backend {
public: public:
AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
void start_bind(void) override; void start_bind(void) override;
void update(void) override; void update(void) override;
private: private:
void _process_byte(uint32_t timestamp_ms, uint8_t byte);
void dsm_decode(); void dsm_decode();
bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]); void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]);
bool dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16], bool dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values,
uint16_t *num_values, uint16_t max_channels);
bool dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values); uint16_t *values, uint16_t *num_values, uint16_t max_values);
uint64_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */ /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */ uint8_t channel_shift;
// state of DSM decoder
struct { // format guessing state
uint16_t bytes[16]; // including start bit and stop bit uint32_t cs10;
uint16_t bit_ofs; uint32_t cs11;
} dsm_state; uint32_t samples;
// bind state machine // bind state machine
enum { enum {
@ -54,5 +59,20 @@ private:
uint32_t bind_last_ms; uint32_t bind_last_ms;
uint16_t last_values[AP_DSM_MAX_CHANNELS]; uint16_t last_values[AP_DSM_MAX_CHANNELS];
uint16_t num_channels;
struct {
uint8_t buf[16];
uint8_t ofs;
} byte_input;
enum DSM_DECODE_STATE {
DSM_DECODE_STATE_DESYNC = 0,
DSM_DECODE_STATE_SYNC
} dsm_decode_state;
uint32_t last_frame_time_ms;
uint32_t last_rx_time_ms;
uint16_t chan_count;
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
}; };

2
libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp
View File

@ -11,7 +11,7 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */

6
libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h
View File

@ -11,15 +11,13 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
#pragma once #pragma once
#include "AP_RCProtocol.h" #include "AP_RCProtocol.h"
#define MAX_PPM_CHANNELS 16
class AP_RCProtocol_PPMSum : public AP_RCProtocol_Backend { class AP_RCProtocol_PPMSum : public AP_RCProtocol_Backend {
public: public:
AP_RCProtocol_PPMSum(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} AP_RCProtocol_PPMSum(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
@ -30,4 +28,4 @@ private:
int8_t _channel_counter; int8_t _channel_counter;
uint16_t _pulse_capt[MAX_RCIN_CHANNELS]; uint16_t _pulse_capt[MAX_RCIN_CHANNELS];
} ppm_state; } ppm_state;
}; };

307
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp
View File

@ -47,7 +47,7 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
@ -82,120 +82,126 @@
* - left shift applied to the result into the channel value * - left shift applied to the result into the channel value
*/ */
struct sbus_bit_pick { struct sbus_bit_pick {
uint8_t byte; uint8_t byte;
uint8_t rshift; uint8_t rshift;
uint8_t mask; uint8_t mask;
uint8_t lshift; uint8_t lshift;
}; };
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
/* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
/* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
/* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
/* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
/* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
/* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
/* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
/* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
/* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
}; };
// constructor
AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted) :
AP_RCProtocol_Backend(_frontend),
inverted(_inverted)
{}
// decode a full SBUS frame
bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
{ {
/* check frame boundary markers to avoid out-of-sync cases */ /* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) { if ((frame[0] != 0x0f)) {
return false; return false;
} }
switch (frame[24]) { switch (frame[24]) {
case 0x00: case 0x00:
/* this is S.BUS 1 */ /* this is S.BUS 1 */
break; break;
case 0x03: case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */ /* S.BUS 2 SLOT0: RX battery and external voltage */
break; break;
case 0x83: case 0x83:
/* S.BUS 2 SLOT1 */ /* S.BUS 2 SLOT1 */
break; break;
case 0x43: case 0x43:
case 0xC3: case 0xC3:
case 0x23: case 0x23:
case 0xA3: case 0xA3:
case 0x63: case 0x63:
case 0xE3: case 0xE3:
break; break;
default: default:
/* we expect one of the bits above, but there are some we don't know yet */ /* we expect one of the bits above, but there are some we don't know yet */
break; break;
} }
unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : max_values; SBUS_INPUT_CHANNELS : max_values;
/* use the decoder matrix to extract channel data */ /* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) { for (unsigned channel = 0; channel < chancount; channel++) {
unsigned value = 0; unsigned value = 0;
for (unsigned pick = 0; pick < 3; pick++) { for (unsigned pick = 0; pick < 3; pick++) {
const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
if (decode->mask != 0) { if (decode->mask != 0) {
unsigned piece = frame[1 + decode->byte]; unsigned piece = frame[1 + decode->byte];
piece >>= decode->rshift; piece >>= decode->rshift;
piece &= decode->mask; piece &= decode->mask;
piece <<= decode->lshift; piece <<= decode->lshift;
value |= piece; value |= piece;
} }
} }
/* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
} }
/* decode switch channels if data fields are wide enough */ /* decode switch channels if data fields are wide enough */
if (max_values > 17 && chancount > 15) { if (max_values > 17 && chancount > 15) {
chancount = 18; chancount = 18;
/* channel 17 (index 16) */ /* channel 17 (index 16) */
values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0))?1998:998;
/* channel 18 (index 17) */ /* channel 18 (index 17) */
values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1))?1998:998;
} }
/* note the number of channels decoded */ /* note the number of channels decoded */
*num_values = chancount; *num_values = chancount;
/* decode and handle failsafe and frame-lost flags */ /* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */ /* report that we failed to read anything valid off the receiver */
*sbus_failsafe = true; *sbus_failsafe = true;
*sbus_frame_drop = true; *sbus_frame_drop = true;
} } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag
/* set a special warning flag *
* * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * condition as fail-safe greatly reduces the reliability and range of the radio link,
* condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issuing return-to-launch!!! */
* e.g. by prematurely issuing return-to-launch!!! */
*sbus_failsafe = false; *sbus_failsafe = false;
*sbus_frame_drop = true; *sbus_frame_drop = true;
} else { } else {
*sbus_failsafe = false; *sbus_failsafe = false;
*sbus_frame_drop = false; *sbus_frame_drop = false;
} }
return true; return true;
} }
@ -204,81 +210,60 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values,
*/ */
void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1) void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1)
{ {
// convert to bit widths, allowing for up to 4usec error, assuming 100000 bps uint32_t w0 = width_s0;
uint16_t bits_s0 = (width_s0+4) / 10; uint32_t w1 = width_s1;
uint16_t bits_s1 = (width_s1+4) / 10; if (inverted) {
uint16_t nlow; w0 = saved_width;
w1 = width_s0;
saved_width = width_s1;
}
uint8_t b;
if (ss.process_pulse(w0, w1, b)) {
_process_byte(ss.get_byte_timestamp_us(), b);
}
}
uint8_t byte_ofs = sbus_state.bit_ofs/12; // support byte input
uint8_t bit_ofs = sbus_state.bit_ofs%12; void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b)
{
const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U);
byte_input.last_byte_us = timestamp_us;
if (bits_s0 == 0 || bits_s1 == 0) { if (have_frame_gap) {
// invalid data // if we have a frame gap then this must be the start of a new
goto reset; // frame
byte_input.ofs = 0;
}
if (b != 0x0F && byte_input.ofs == 0) {
// definately not SBUS, missing header byte
return;
}
if (byte_input.ofs == 0 && !have_frame_gap) {
// must have a frame gap before the start of a new SBUS frame
return;
} }
if (bits_s0+bit_ofs > 10) { byte_input.buf[byte_input.ofs++] = b;
// invalid data as last two bits must be stop bits
goto reset;
}
if (byte_ofs >= ARRAY_SIZE(sbus_state.bytes)) { if (byte_input.ofs == sizeof(byte_input.buf)) {
goto reset; uint16_t values[SBUS_INPUT_CHANNELS];
}
// pull in the high bits
sbus_state.bytes[byte_ofs] |= ((1U<<bits_s0)-1) << bit_ofs;
sbus_state.bit_ofs += bits_s0;
bit_ofs += bits_s0;
// pull in the low bits
nlow = bits_s1;
if (nlow + bit_ofs > 12) {
nlow = 12 - bit_ofs;
}
bits_s1 -= nlow;
sbus_state.bit_ofs += nlow;
if (sbus_state.bit_ofs == 25*12 && bits_s1 > 12) {
// we have a full frame
uint8_t bytes[25];
uint8_t i;
for (i=0; i<25; i++) {
// get inverted data
uint16_t v = ~sbus_state.bytes[i];
// check start bit
if ((v & 1) != 0) {
goto reset;
}
// check stop bits
if ((v & 0xC00) != 0xC00) {
goto reset;
}
// check parity
uint8_t parity = 0, j;
for (j=1; j<=8; j++) {
parity ^= (v & (1U<<j))?1:0;
}
if (parity != (v&0x200)>>9) {
goto reset;
}
bytes[i] = ((v>>1) & 0xFF);
}
uint16_t values[MAX_RCIN_CHANNELS];
uint16_t num_values=0; uint16_t num_values=0;
bool sbus_failsafe=false, sbus_frame_drop=false; bool sbus_failsafe = false;
if (sbus_decode(bytes, values, &num_values, bool sbus_frame_drop = false;
&sbus_failsafe, &sbus_frame_drop, if (sbus_decode(byte_input.buf, values, &num_values,
MAX_RCIN_CHANNELS) && &sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) &&
num_values >= MIN_RCIN_CHANNELS) { num_values >= MIN_RCIN_CHANNELS) {
add_input(num_values, values, sbus_failsafe); add_input(num_values, values, sbus_failsafe);
} }
goto reset; byte_input.ofs = 0;
} else if (bits_s1 > 12) {
// break
goto reset;
} }
return; }
reset:
memset(&sbus_state, 0, sizeof(sbus_state)); // support byte input
void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate)
{
if (baudrate != 100000) {
return;
}
_process_byte(AP_HAL::micros(), b);
} }

23
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h
View File

@ -11,23 +11,32 @@
* *
* You should have received a copy of the GNU General Public License along * You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>. * with this program. If not, see <http://www.gnu.org/licenses/>.
* *
* Code by Andrew Tridgell and Siddharth Bharat Purohit * Code by Andrew Tridgell and Siddharth Bharat Purohit
*/ */
#pragma once #pragma once
#include "AP_RCProtocol.h" #include "AP_RCProtocol.h"
#include "SoftSerial.h"
class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend { class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend {
public: public:
AP_RCProtocol_SBUS(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool inverted);
void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private: private:
void _process_byte(uint32_t timestamp_us, uint8_t byte);
bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values);
bool inverted;
SoftSerial ss{100000, SoftSerial::SERIAL_CONFIG_8E2I};
uint32_t saved_width;
struct { struct {
uint16_t bytes[25]; // including start bit, parity and stop bits uint8_t buf[25];
uint16_t bit_ofs; uint8_t ofs;
} sbus_state; uint32_t last_byte_us;
}; } byte_input;
};

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

@ -0,0 +1,301 @@
/*
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)
{
uint8_t b;
if (ss.process_pulse(width_s0, width_s1, b)) {
_process_byte(ss.get_byte_timestamp_us(), b);
}
}
/**
* 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(uint32_t timestamp_us, uint8_t byte)
{
/*----------------------------------------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;
}
/*
process a byte provided by a uart
*/
void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(AP_HAL::micros(), byte);
}

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

@ -0,0 +1,67 @@
/*
* 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"
#include "SoftSerial.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, uint32_t baudrate) override;
private:
void _process_byte(uint32_t timestamp_us, uint8_t byte);
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 */
uint32_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 */
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
};

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

@ -0,0 +1,233 @@
/*
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)
{
uint8_t b;
if (ss.process_pulse(width_s0, width_s1, b)) {
_process_byte(b);
}
}
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;
}
}
void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(byte);
}

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

@ -0,0 +1,151 @@
/*
* 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"
#include "SoftSerial.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, uint32_t baudrate) override;
private:
void _process_byte(uint8_t byte);
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;
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
};

344
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp Normal file
View File

@ -0,0 +1,344 @@
/*
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_SUMD.h"
#define SUMD_HEADER_LENGTH 3
#define SUMD_HEADER_ID 0xA8
#define SUMD_ID_SUMH 0x00
#define SUMD_ID_SUMD 0x01
#define SUMD_ID_FAILSAFE 0x81
/* define range mapping here, -+100% -> 1000..2000 */
#define SUMD_RANGE_MIN 0.0f
#define SUMD_RANGE_MAX 4096.0f
#define SUMD_TARGET_MIN 1000.0f
#define SUMD_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define SUMD_SCALE_FACTOR ((SUMD_TARGET_MAX - SUMD_TARGET_MIN) / (SUMD_RANGE_MAX - SUMD_RANGE_MIN))
#define SUMD_SCALE_OFFSET (int)(SUMD_TARGET_MIN - (SUMD_SCALE_FACTOR * SUMD_RANGE_MIN + 0.5f))
// #define SUMD_DEBUG
extern const AP_HAL::HAL& hal;
uint16_t AP_RCProtocol_SUMD::sumd_crc16(uint16_t crc, uint8_t value)
{
int i;
crc ^= (uint16_t)value << 8;
for (i = 0; i < 8; i++) {
crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1);
}
return crc;
}
uint8_t AP_RCProtocol_SUMD::sumd_crc8(uint8_t crc, uint8_t value)
{
crc += value;
return crc;
}
void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
uint8_t b;
if (ss.process_pulse(width_s0, width_s1, b)) {
_process_byte(ss.get_byte_timestamp_us(), b);
}
}
void AP_RCProtocol_SUMD::_process_byte(uint32_t timestamp_us, uint8_t byte)
{
if (timestamp_us - last_packet_us > 3000U) {
_decode_state = SUMD_DECODE_STATE_UNSYNCED;
}
switch (_decode_state) {
case SUMD_DECODE_STATE_UNSYNCED:
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_UNSYNCED \n") ;
#endif
if (byte == SUMD_HEADER_ID) {
_rxpacket.header = byte;
_sumd = true;
_rxlen = 0;
_crc16 = 0x0000;
_crc8 = 0x00;
_crcOK = false;
_crc16 = sumd_crc16(_crc16, byte);
_crc8 = sumd_crc8(_crc8, byte);
_decode_state = SUMD_DECODE_STATE_GOT_HEADER;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ;
#endif
last_packet_us = timestamp_us;
}
break;
case SUMD_DECODE_STATE_GOT_HEADER:
if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) {
_rxpacket.status = byte;
if (byte == SUMD_ID_SUMH) {
_sumd = false;
}
if (_sumd) {
_crc16 = sumd_crc16(_crc16, byte);
} else {
_crc8 = sumd_crc8(_crc8, byte);
}
_decode_state = SUMD_DECODE_STATE_GOT_STATE;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ;
#endif
} else {
_decode_state = SUMD_DECODE_STATE_UNSYNCED;
}
break;
case SUMD_DECODE_STATE_GOT_STATE:
if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) {
_rxpacket.length = byte;
if (_sumd) {
_crc16 = sumd_crc16(_crc16, byte);
} else {
_crc8 = sumd_crc8(_crc8, byte);
}
_rxlen++;
_decode_state = SUMD_DECODE_STATE_GOT_LEN;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ;
#endif
} else {
_decode_state = SUMD_DECODE_STATE_UNSYNCED;
}
break;
case SUMD_DECODE_STATE_GOT_LEN:
_rxpacket.sumd_data[_rxlen] = byte;
if (_sumd) {
_crc16 = sumd_crc16(_crc16, byte);
} else {
_crc8 = sumd_crc8(_crc8, byte);
}
_rxlen++;
if (_rxlen <= ((_rxpacket.length * 2))) {
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ;
#endif
} else {
_decode_state = SUMD_DECODE_STATE_GOT_DATA;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ;
#endif
}
break;
case SUMD_DECODE_STATE_GOT_DATA:
_rxpacket.crc16_high = byte;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ;
#endif
if (_sumd) {
_decode_state = SUMD_DECODE_STATE_GOT_CRC;
} else {
_decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1;
}
break;
case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1:
_rxpacket.crc16_low = byte;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ;
#endif
_decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2;
break;
case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2:
_rxpacket.telemetry = byte;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ;
#endif
_decode_state = SUMD_DECODE_STATE_GOT_CRC;
break;
case SUMD_DECODE_STATE_GOT_CRC:
if (_sumd) {
_rxpacket.crc16_low = byte;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ;
#endif
if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) {
_crcOK = true;
}
} else {
_rxpacket.crc8 = byte;
#ifdef SUMD_DEBUG
hal.console->printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ;
#endif
if (_crc8 == _rxpacket.crc8) {
_crcOK = true;
}
}
if (_crcOK) {
#ifdef SUMD_DEBUG
hal.console->printf(" CRC - OK \n") ;
#endif
if (_sumd) {
#ifdef SUMD_DEBUG
hal.console->printf(" Got valid SUMD Packet\n") ;
#endif
} else {
#ifdef SUMD_DEBUG
hal.console->printf(" Got valid SUMH Packet\n") ;
#endif
}
#ifdef SUMD_DEBUG
hal.console->printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ;
#endif
unsigned i;
uint8_t num_values;
uint16_t values[SUMD_MAX_CHANNELS];
/* received Channels */
if ((uint16_t)_rxpacket.length > SUMD_MAX_CHANNELS) {
_rxpacket.length = (uint8_t) SUMD_MAX_CHANNELS;
}
num_values = (uint16_t)_rxpacket.length;
/* decode the actual packet */
/* reorder first 4 channels */
/* ch1 = roll -> sumd = ch2 */
values[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3;
/* ch2 = pitch -> sumd = ch2 */
values[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3;
/* ch3 = throttle -> sumd = ch2 */
values[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3;
/* ch4 = yaw -> sumd = ch2 */
values[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3;
/* we start at channel 5(index 4) */
unsigned chan_index = 4;
for (i = 4; i < _rxpacket.length; i++) {
#ifdef SUMD_DEBUG
hal.console->printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2],
((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3,
((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3);
#endif
values[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3;
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
//channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET;
chan_index++;
}
if (_rxpacket.status == 0x01) {
add_input(num_values, values, false);
} else if (_rxpacket.status == 0x81) {
add_input(num_values, values, true);
}
} else {
#ifdef SUMD_DEBUG
hal.console->printf(" CRC - fail 0x%X 0x%X\n", _crc16, (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low);
#endif
}
_decode_state = SUMD_DECODE_STATE_UNSYNCED;
break;
}
}
void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(AP_HAL::micros(), byte);
}

71
libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h Normal file
View File

@ -0,0 +1,71 @@
/*
* 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"
#include "SoftSerial.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) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
void process_byte(uint8_t byte, uint32_t baudrate) override;
private:
void _process_byte(uint32_t timestamp_us, uint8_t byte);
static uint16_t sumd_crc16(uint16_t crc, uint8_t value);
static uint8_t sumd_crc8(uint8_t crc, uint8_t value);
#pragma pack(push, 1)
typedef struct {
uint8_t header; ///< 0xA8 for a valid packet
uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe
uint8_t length; ///< Channels
uint8_t sumd_data[(SUMD_MAX_CHANNELS+1) * 2]; ///< ChannelData (High Byte/ Low Byte)
uint8_t crc16_high; ///< High Byte of 16 Bit CRC
uint8_t crc16_low; ///< Low Byte of 16 Bit CRC
uint8_t telemetry; ///< Telemetry request
uint8_t crc8; ///< SUMH CRC8
} ReceiverFcPacketHoTT;
#pragma pack(pop)
enum SUMD_DECODE_STATE {
SUMD_DECODE_STATE_UNSYNCED = 0,
SUMD_DECODE_STATE_GOT_HEADER,
SUMD_DECODE_STATE_GOT_STATE,
SUMD_DECODE_STATE_GOT_LEN,
SUMD_DECODE_STATE_GOT_DATA,
SUMD_DECODE_STATE_GOT_CRC,
SUMD_DECODE_STATE_GOT_CRC16_BYTE_1,
SUMD_DECODE_STATE_GOT_CRC16_BYTE_2
};
enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED;
uint8_t _rxlen;
ReceiverFcPacketHoTT _rxpacket;
uint8_t _crc8 = 0x00;
uint16_t _crc16 = 0x0000;
bool _sumd = true;
bool _crcOK = false;
uint32_t last_packet_us;
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
};

114
libraries/AP_RCProtocol/SoftSerial.cpp Normal file
View File

@ -0,0 +1,114 @@
/*
* 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/>.
*/
/*
soft serial receive implementation, based on pulse width inputs
*/
#include "SoftSerial.h"
#include <stdio.h>
SoftSerial::SoftSerial(uint32_t _baudrate, serial_config _config) :
baudrate(_baudrate),
config(_config),
half_bit((1000000U / baudrate)/2)
{
switch (config) {
case SERIAL_CONFIG_8N1:
data_width = 8;
byte_width = 10;
stop_mask = 0x200;
break;
case SERIAL_CONFIG_8E2I:
data_width = 9;
byte_width = 12;
stop_mask = 0xC00;
break;
}
}
/*
process a pulse made up of a width of values at high voltage
followed by a width at low voltage
*/
bool SoftSerial::process_pulse(uint32_t width_high, uint32_t width_low, uint8_t &byte)
{
// convert to bit widths, allowing for a half bit error
uint16_t bits_high = ((width_high+half_bit)*baudrate) / 1000000;
uint16_t bits_low = ((width_low+half_bit)*baudrate) / 1000000;
byte_timestamp_us = timestamp_us;
timestamp_us += (width_high + width_low);
if (bits_high == 0 || bits_low == 0) {
// invalid data
goto reset;
}
if (bits_high >= byte_width) {
// if we have a start bit and a stop bit then we can have at
// most 9 bits in high state for data. The rest must be idle
// bits
bits_high = byte_width-1;
}
if (state.bit_ofs == 0) {
// we are in idle state, waiting for first low bit. swallow
// the high bits
bits_high = 0;
}
state.byte |= ((1U<<bits_high)-1) << state.bit_ofs;
state.bit_ofs += bits_high + bits_low;
if (state.bit_ofs >= byte_width) {
// check start bit
if ((state.byte & 1) != 0) {
goto reset;
}
// check stop bits
if ((state.byte & stop_mask) != stop_mask) {
goto reset;
}
if (config == SERIAL_CONFIG_8E2I) {
// check parity
if (__builtin_parity((state.byte>>1)&0xFF) != (state.byte&0x200)>>9) {
goto reset;
}
}
byte = ((state.byte>>1) & 0xFF);
state.byte >>= byte_width;
state.bit_ofs -= byte_width;
if (state.bit_ofs > byte_width) {
state.byte = 0;
state.bit_ofs = bits_low;
}
// swallow idle bits
while (state.bit_ofs > 0 && (state.byte & 1)) {
state.bit_ofs--;
state.byte >>= 1;
}
return true;
}
return false;
reset:
state.byte = 0;
state.bit_ofs = 0;
return false;
}

50
libraries/AP_RCProtocol/SoftSerial.h Normal file
View File

@ -0,0 +1,50 @@
/*
* 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"
class SoftSerial {
public:
enum serial_config {
SERIAL_CONFIG_8N1, // DSM, SRXL etc, 8 bit, no parity, 1 stop bit
SERIAL_CONFIG_8E2I, // SBUS, 8 bit, even parity, 2 stop bits, inverted
};
SoftSerial(uint32_t baudrate, enum serial_config config);
bool process_pulse(uint32_t width_s0, uint32_t width_s1, uint8_t &b);
// get timestamp of the last byte
uint32_t get_byte_timestamp_us(void) const {
return byte_timestamp_us;
}
private:
const uint32_t baudrate;
const uint8_t half_bit; // width of half a bit in microseconds
const enum serial_config config;
uint8_t data_width;
uint8_t byte_width;
uint16_t stop_mask;
uint32_t timestamp_us;
uint32_t byte_timestamp_us;
struct {
uint32_t byte;
uint16_t bit_ofs;
} state;
};