Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

allow for any valid SBUS valid end byte #57

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

allow for any valid SBUS valid end byte #57

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 29 additions & 29 deletions include/rc_sbus.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,53 +42,55 @@ class RC_SBUS : public RC_BASE
{

private:

enum
{
START_BYTE = 0x0F,
END_BYTE = 0x00
END_MASK = 0x34
};

inline bool is_end_byte(uint8_t byte) { return (byte & END_MASK) == byte; }

typedef enum
{
SBUS_SIGNAL_OK,
SBUS_SIGNAL_LOST,
SBUS_SIGNAL_FAILSAFE
} failsafe_state_t;

struct dataFrame_s {
uint8_t startByte;
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
unsigned int chan3 : 11;
unsigned int chan4 : 11;
unsigned int chan5 : 11;
unsigned int chan6 : 11;
unsigned int chan7 : 11;
unsigned int chan8 : 11;
unsigned int chan9 : 11;
unsigned int chan10 : 11;
unsigned int chan11 : 11;
unsigned int chan12 : 11;
unsigned int chan13 : 11;
unsigned int chan14 : 11;
unsigned int chan15 : 11;
uint8_t digichannels;
uint8_t endByte;
} __attribute__ ((__packed__));
struct dataFrame_s
{
uint8_t startByte;
unsigned int chan0 : 11;
unsigned int chan1 : 11;
unsigned int chan2 : 11;
unsigned int chan3 : 11;
unsigned int chan4 : 11;
unsigned int chan5 : 11;
unsigned int chan6 : 11;
unsigned int chan7 : 11;
unsigned int chan8 : 11;
unsigned int chan9 : 11;
unsigned int chan10 : 11;
unsigned int chan11 : 11;
unsigned int chan12 : 11;
unsigned int chan13 : 11;
unsigned int chan14 : 11;
unsigned int chan15 : 11;
uint8_t digichannels;
uint8_t endByte;
} __attribute__((__packed__));

typedef union {
uint8_t data[25];
struct dataFrame_s frame;
uint8_t data[25];
struct dataFrame_s frame;
} SBUS_t;

SBUS_t sbus_union_;

failsafe_state_t failsafe_status_;

GPIO* inv_pin_;
UART* uart_;
GPIO *inv_pin_;
UART *uart_;
uint32_t raw_[18];
uint32_t frame_start_ms_ = 0;
uint8_t buffer_[25];
Expand All @@ -100,13 +102,11 @@ class RC_SBUS : public RC_BASE
void decode_buffer();

public:

void init(GPIO *inv_pin, UART *uart);
void read_cb(uint8_t byte);
float read(uint8_t channel);
bool lost();
inline uint32_t get_errors() { return errors_; }

};

#endif // RC_SBUS_H
44 changes: 21 additions & 23 deletions src/rc_sbus.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,17 +29,16 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/


#include "rc_sbus.h"
#include <functional>

RC_SBUS* rc_ptr;
RC_SBUS *rc_ptr;
void rx_callback(uint8_t byte)
{
rc_ptr->read_cb(byte);
rc_ptr->read_cb(byte);
}

void RC_SBUS::init(GPIO* inv_pin, UART *uart)
void RC_SBUS::init(GPIO *inv_pin, UART *uart)
{
rc_ptr = this;
uart_ = uart;
Expand All @@ -56,7 +55,7 @@ void RC_SBUS::init(GPIO* inv_pin, UART *uart)

float RC_SBUS::read(uint8_t channel)
{
return (static_cast<float>(raw_[channel]) - 172.0)/1639.0;
return (static_cast<float>(raw_[channel]) - 172.0) / 1639.0;
}

bool RC_SBUS::lost()
Expand All @@ -69,16 +68,16 @@ void RC_SBUS::decode_buffer()
frame_start_ms_ = millis();

// process actual sbus data, use union to decode
raw_[0] = sbus_union_.frame.chan0;
raw_[1] = sbus_union_.frame.chan1;
raw_[2] = sbus_union_.frame.chan2;
raw_[3] = sbus_union_.frame.chan3;
raw_[4] = sbus_union_.frame.chan4;
raw_[5] = sbus_union_.frame.chan5;
raw_[6] = sbus_union_.frame.chan6;
raw_[7] = sbus_union_.frame.chan7;
raw_[8] = sbus_union_.frame.chan8;
raw_[9] = sbus_union_.frame.chan9;
raw_[0] = sbus_union_.frame.chan0;
raw_[1] = sbus_union_.frame.chan1;
raw_[2] = sbus_union_.frame.chan2;
raw_[3] = sbus_union_.frame.chan3;
raw_[4] = sbus_union_.frame.chan4;
raw_[5] = sbus_union_.frame.chan5;
raw_[6] = sbus_union_.frame.chan6;
raw_[7] = sbus_union_.frame.chan7;
raw_[8] = sbus_union_.frame.chan8;
raw_[9] = sbus_union_.frame.chan9;
raw_[10] = sbus_union_.frame.chan10;
raw_[11] = sbus_union_.frame.chan11;
raw_[12] = sbus_union_.frame.chan12;
Expand All @@ -87,28 +86,28 @@ void RC_SBUS::decode_buffer()
raw_[15] = sbus_union_.frame.chan15;

// Digital Channel 1
if (sbus_union_.frame.digichannels & (1<<0))
if (sbus_union_.frame.digichannels & (1 << 0))
raw_[16] = 1811;
else
raw_[16] = 172;

// Digital Channel 2
if (sbus_union_.frame.digichannels & (1<<1))
if (sbus_union_.frame.digichannels & (1 << 1))
raw_[17] = 1811;
else
raw_[17] = 172;

// Failsafe
failsafe_status_ = SBUS_SIGNAL_OK;
// if (sbus_union_.frame.digichannels & (1<<2))
// failsafe_status_ = SBUS_SIGNAL_LOST;
if (sbus_union_.frame.digichannels & (1<<3))
// if (sbus_union_.frame.digichannels & (1<<2))
// failsafe_status_ = SBUS_SIGNAL_LOST;
if (sbus_union_.frame.digichannels & (1 << 3))
failsafe_status_ = SBUS_SIGNAL_FAILSAFE;
}

void RC_SBUS::read_cb(uint8_t byte)
{
if (byte == START_BYTE && prev_byte_ == END_BYTE)
if (byte == START_BYTE && is_end_byte(prev_byte_))
{
buffer_pos_ = 0;
frame_started_ = true;
Expand All @@ -122,7 +121,7 @@ void RC_SBUS::read_cb(uint8_t byte)
if (buffer_pos_ == 25)
{
// If we have a complete packet, decode
if (sbus_union_.frame.endByte == END_BYTE && sbus_union_.frame.startByte == START_BYTE)
if (is_end_byte(sbus_union_.frame.endByte) && sbus_union_.frame.startByte == START_BYTE)
decode_buffer();
else
errors_++;
Expand All @@ -131,4 +130,3 @@ void RC_SBUS::read_cb(uint8_t byte)
}
prev_byte_ = byte;
}