aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RCProtocol: add library to process RC signals

Browse Source
This commit is contained in:
bugobliterator 2018-01-18 01:01:11 +05:30 committed by Andrew Tridgell
parent eeea2c9961
commit bad9f2b4c9
10 changed files with 1017 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_RCProtocol/AP_RCProtocol.cpp Normal file
View File

@ -0,0 +1,66 @@
/*
* 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
*/
#include "AP_RCProtocol.h"
#include "AP_RCProtocol_PPMSum.h"
#include "AP_RCProtocol_DSM.h"
#include "AP_RCProtocol_SBUS.h"
void AP_RCProtocol::init()
{
backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this);
backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this);
backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this);
}
void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
if (backend[i] != nullptr) {
backend[i]->process_pulse(width_s0, width_s1);
if (backend[i]->new_input()) {
_new_input = true;
_detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i;
}
}
}
}
bool AP_RCProtocol::new_input()
{
bool ret = _new_input;
_new_input = false;
return ret;
}
uint8_t AP_RCProtocol::num_channels()
{
if (_detected_protocol != AP_RCProtocol::NONE) {
return backend[_detected_protocol]->num_channels();
} else {
return AP_RCProtocol::NONE;
}
}
uint16_t AP_RCProtocol::read(uint8_t chan)
{
if (_detected_protocol != AP_RCProtocol::NONE) {
return backend[_detected_protocol]->read(chan);
} else {
return 0;
}
}

46
libraries/AP_RCProtocol/AP_RCProtocol.h Normal file
View File

@ -0,0 +1,46 @@
/*
* 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_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#define MAX_RCIN_CHANNELS 16
#define MIN_RCIN_CHANNELS 5
class AP_RCProtocol_Backend;
class AP_RCProtocol {
public:
enum rcprotocol_t{
PPM = 0,
SBUS,
DSM,
NONE //last enum always is None
};
void init();
void process_pulse(uint32_t width_s0, uint32_t width_s1);
enum rcprotocol_t protocol_detected() { return _detected_protocol; }
uint8_t num_channels();
uint16_t read(uint8_t chan);
bool new_input();
private:
enum rcprotocol_t _detected_protocol = NONE;
AP_RCProtocol_Backend *backend[NONE];
uint8_t num_backends = NONE;
bool _new_input = false;
};
#include "AP_RCProtocol_Backend.h"

45
libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp Normal file
View File

@ -0,0 +1,45 @@
/*
* 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
*/
#include "AP_RCProtocol.h"
AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) :
frontend(_frontend),
rc_input_count(0),
last_rc_input_count(0),
_num_channels(0)
{}
bool AP_RCProtocol_Backend::new_input()
{
bool ret = rc_input_count != last_rc_input_count;
if (ret) {
last_rc_input_count = rc_input_count;
}
return ret;
}
uint8_t AP_RCProtocol_Backend::num_channels()
{
return _num_channels;
}
uint16_t AP_RCProtocol_Backend::read(uint8_t chan)
{
return _pwm_values[chan];
}

39
libraries/AP_RCProtocol/AP_RCProtocol_Backend.h Normal file
View File

@ -0,0 +1,39 @@
/*
* 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"
class AP_RCProtocol_Backend
{
friend class AP_RCProtcol;
public:
AP_RCProtocol_Backend(AP_RCProtocol &_frontend);
virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) = 0;
uint16_t read(uint8_t chan);
bool new_input();
uint8_t num_channels();
protected:
AP_RCProtocol &frontend;
unsigned int rc_input_count;
unsigned int last_rc_input_count;
uint16_t _pwm_values[MAX_RCIN_CHANNELS] = {0};
uint8_t _num_channels;
};

361
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp Normal file
View File

@ -0,0 +1,361 @@
/*
* 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
*/
#include "AP_RCProtocol_DSM.h"
//#define DEBUG
#ifdef DEBUG
# define debug(fmt, args...) printf(fmt "\n", ##args)
#else
# define debug(fmt, args...) do {} while(0)
#endif
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
// convert to bit widths, allowing for up to 1usec 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 = 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);
}
uint16_t values[8];
uint16_t num_values=0;
if (dsm_decode(AP_HAL::micros64(), bytes, values, &num_values, 8) &&
num_values >= MIN_RCIN_CHANNELS) {
for (i=0; i<num_values; i++) {
_pwm_values[i] = values[i];
}
_num_channels = num_values;
}
}
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));
}
/**
* Attempt to decode a single channel raw channel datum
*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect dsm frame boundaries by the inter-dsm frame delay.
*
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
* dsm frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a dsm frame.
*
* In the case where byte(s) are dropped from a dsm frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*
* Upon receiving a full dsm frame we attempt to decode it
*
* @param[in] raw 16 bit raw channel value from dsm frame
* @param[in] shift position of channel number in raw data
* @param[out] channel pointer to returned channel number
* @param[out] value pointer to returned channel value
* @return true=raw value successfully decoded
*/
bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{
if (raw == 0xffff)
return false;
*channel = (raw >> shift) & 0xf;
uint16_t data_mask = (1 << shift) - 1;
*value = raw & data_mask;
//debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value);
return true;
}
/**
* Attempt to guess if receiving 10 or 11 bit channel values
*
* @param[in] reset true=reset the 10/11 bit state to unknown
*/
void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16])
{
static uint32_t cs10;
static uint32_t cs11;
static unsigned samples;
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
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 */
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
const uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
cs10 |= (1 << channel);
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
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;
/*
* 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 (cs11 == masks[i])
votes11++;
}
if ((votes11 == 1) && (votes10 == 0)) {
dsm_channel_shift = 11;
debug("DSM: 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 10;
debug("DSM: 10-bit format");
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)
*
*/
bool AP_RCProtocol_DSM::dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values)
{
/*
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
*/
/*
* 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 */
dsm_last_frame_time = frame_time;
/* if we don't know the dsm_frame format, update the guessing state machine */
if (dsm_channel_shift == 0) {
dsm_guess_format(false, dsm_frame);
return false;
}
/*
* The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now.
*
* Each channel is a 16-bit unsigned value containing either a 10-
* 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
* second dsm_frame in variants of the protocol where more than
* seven channels are being transmitted.
*/
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
const uint8_t *dp = &dsm_frame[2 + (2 * i)];
uint16_t raw = (dp[0] << 8) | dp[1];
unsigned channel, value;
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
continue;
/* ignore channels out of range */
if (channel >= max_values)
continue;
/* update the decoded channel count */
if (channel >= *num_values)
*num_values = channel + 1;
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
if (dsm_channel_shift == 10)
value *= 2;
/*
* Spektrum scaling is special. There are these basic considerations
*
* * Midpoint is 1520 us
* * 100% travel channels are +- 400 us
*
* 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
* 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
* link for no reason.
*/
/* scaled integer for decent accuracy while staying efficient */
value = ((((int)value - 1024) * 1000) / 1700) + 1500;
/*
* Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have.
*
* 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.
*/
switch (channel) {
case 0:
channel = 2;
break;
case 1:
channel = 0;
break;
case 2:
channel = 1;
default:
break;
}
values[channel] = value;
}
/*
* 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
* lines, so if we get a channel count of 13, we'll return 12 (the last
* data index that is stable).
*/
if (*num_values == 13)
*num_values = 12;
#if 0
if (dsm_channel_shift == 11) {
/* Set the 11-bit data indicator */
*num_values |= 0x8000;
}
#endif
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/
return true;
}

40
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h Normal file
View File

@ -0,0 +1,40 @@
/*
* 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"
class AP_RCProtocol_DSM : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
private:
void dsm_decode();
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]);
bool dsm_decode(uint64_t frame_time, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values);
uint64_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
// state of DSM decoder
struct {
uint16_t bytes[16]; // including start bit and stop bit
uint16_t bit_ofs;
} dsm_state;
};

68
libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.cpp Normal file
View File

@ -0,0 +1,68 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#include "AP_RCProtocol_PPMSum.h"
/*
process a PPM-sum pulse of the given width
*/
void AP_RCProtocol_PPMSum::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
uint32_t width_usec = width_s0 + width_s1;
if (width_usec >= 2700) {
// a long pulse indicates the end of a frame. Reset the
// channel counter so next pulse is channel 0
if (ppm_state._channel_counter >= MIN_RCIN_CHANNELS) {
for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
_pwm_values[i] = ppm_state._pulse_capt[i];
}
_num_channels = ppm_state._channel_counter;
rc_input_count++;
}
ppm_state._channel_counter = 0;
return;
}
if (ppm_state._channel_counter == -1) {
// we are not synchronised
return;
}
/*
we limit inputs to between 700usec and 2300usec. This allows us
to decode SBUS on the same pin, as SBUS will have a maximum
pulse width of 100usec
*/
if (width_usec > 700 && width_usec < 2300) {
// take a reading for the current channel
// buffer these
ppm_state._pulse_capt[ppm_state._channel_counter] = width_usec;
// move to next channel
ppm_state._channel_counter++;
}
// if we have reached the maximum supported channels then
// mark as unsynchronised, so we wait for a wide pulse
if (ppm_state._channel_counter >= MAX_RCIN_CHANNELS) {
for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
_pwm_values[i] = ppm_state._pulse_capt[i];
}
_num_channels = ppm_state._channel_counter;
rc_input_count++;
ppm_state._channel_counter = -1;
}
}

33
libraries/AP_RCProtocol/AP_RCProtocol_PPMSum.h Normal file
View File

@ -0,0 +1,33 @@
/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#pragma once
#include "AP_RCProtocol.h"
#define MAX_PPM_CHANNELS 16
class AP_RCProtocol_PPMSum : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_PPMSum(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
private:
// state of ppm decoder
struct {
int8_t _channel_counter;
uint16_t _pulse_capt[MAX_RCIN_CHANNELS];
} ppm_state;
};

286
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp Normal file
View File

@ -0,0 +1,286 @@
/*
SBUS decoder, based on src/modules/px4iofirmware/sbus.c from PX4Firmware
modified for use in AP_HAL_* by Andrew Tridgell
*/
/****************************************************************************
*
* Copyright (c) 2012-2014 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.
*
****************************************************************************/
/*
* 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
*/
#include "AP_RCProtocol_SBUS.h"
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
/* define range mapping here, -+100% -> 1000..2000 */
#define SBUS_RANGE_MIN 200.0f
#define SBUS_RANGE_MAX 1800.0f
#define SBUS_TARGET_MIN 1000.0f
#define SBUS_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
/*
* S.bus decoder matrix.
*
* Each channel value can come from up to 3 input bytes. Each row in the
* matrix describes up to three bytes, and each entry gives:
*
* - byte offset in the data portion of the frame
* - right shift applied to the data byte
* - mask for the data byte
* - left shift applied to the result into the channel value
*/
struct sbus_bit_pick {
uint8_t byte;
uint8_t rshift;
uint8_t mask;
uint8_t lshift;
};
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} },
/* 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} },
/* 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} },
/* 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} },
/* 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} },
/* 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} },
/* 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} },
/* 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} },
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
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)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) {
return false;
}
switch (frame[24]) {
case 0x00:
/* this is S.BUS 1 */
break;
case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
case 0x83:
/* S.BUS 2 SLOT1 */
break;
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
break;
default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : max_values;
/* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) {
unsigned value = 0;
for (unsigned pick = 0; pick < 3; pick++) {
const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
if (decode->mask != 0) {
unsigned piece = frame[1 + decode->byte];
piece >>= decode->rshift;
piece &= decode->mask;
piece <<= decode->lshift;
value |= piece;
}
}
/* 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;
}
/* decode switch channels if data fields are wide enough */
if (max_values > 17 && chancount > 15) {
chancount = 18;
/* channel 17 (index 16) */
values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
*num_values = chancount;
/* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */
*sbus_failsafe = true;
*sbus_frame_drop = true;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag
*
* 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,
* e.g. by prematurely issuing return-to-launch!!! */
*sbus_failsafe = false;
*sbus_frame_drop = true;
} else {
*sbus_failsafe = false;
*sbus_frame_drop = false;
}
return true;
}
/*
process a SBUS input pulse of the given width
*/
void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
// convert to bit widths, allowing for up to 1usec error, assuming 100000 bps
uint16_t bits_s0 = (width_s0+1) / 10;
uint16_t bits_s1 = (width_s1+1) / 10;
uint16_t nlow;
uint8_t byte_ofs = sbus_state.bit_ofs/12;
uint8_t bit_ofs = sbus_state.bit_ofs%12;
if (bits_s0 == 0 || bits_s1 == 0) {
// invalid data
goto reset;
}
if (bits_s0+bit_ofs > 10) {
// invalid data as last two bits must be stop bits
goto reset;
}
// 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;
bool sbus_failsafe=false, sbus_frame_drop=false;
if (sbus_decode(bytes, values, &num_values,
&sbus_failsafe, &sbus_frame_drop,
MAX_RCIN_CHANNELS) &&
num_values >= MIN_RCIN_CHANNELS) {
for (i=0; i<num_values; i++) {
_pwm_values[i] = values[i];
}
_num_channels = num_values;
if (!sbus_failsafe) {
rc_input_count++;
}
}
goto reset;
} else if (bits_s1 > 12) {
// break
goto reset;
}
return;
reset:
memset(&sbus_state, 0, sizeof(sbus_state));
}

33
libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h Normal file
View File

@ -0,0 +1,33 @@
/*
* 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"
class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend {
public:
AP_RCProtocol_SBUS(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
private:
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);
struct {
uint16_t bytes[25]; // including start bit, parity and stop bits
uint16_t bit_ofs;
} sbus_state;
};