Hello!
I was wondering if the Navigator board had any tricks for extracting RC inputs from SBUS under the hood? I’ve tried a collection of different programs/templates both in C++ and in Python.
- sbus/src/sbus.cpp at main · bolderflight/sbus · GitHub
- SBUS/src/SBUS.cpp at master · TheDIYGuy999/SBUS · GitHub
The code I’m currently using:
#include <iostream>
#include <cstdint>
#include <cstring>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <chrono>
namespace bfs {
constexpr uint8_t SBUS_HEADER = 0x0F;
constexpr uint8_t SBUS_FOOTER = 0x00;
constexpr uint8_t SBUS2_MASK = 0x04;
constexpr uint8_t SBUS2_FOOTER = 0x04;
constexpr uint8_t CH17_MASK = 0x01;
constexpr uint8_t CH18_MASK = 0x02;
constexpr uint8_t LOST_FRAME_MASK = 0x04;
constexpr uint8_t FAILSAFE_MASK = 0x08;
constexpr uint8_t PAYLOAD_SIZE = 24;
constexpr uint32_t SBUS_TIMEOUT_US = 5000;
struct SBusData {
int16_t ch[16];
bool ch17;
bool ch18;
bool failsafe;
bool lostFrame;
};
class SbusRx {
public:
explicit SbusRx(const char* port) : port_(port) {}
bool Begin();
bool Read();
const SBusData& data() const { return data_; }
private:
bool Parse();
const char* port_;
int fd_;
SBusData data_;
uint8_t buf_[25];
};
bool SbusRx::Begin() {
fd_ = open(port_, O_RDONLY | O_NOCTTY | O_NONBLOCK);
if (fd_ < 0) {
std::cerr << "Failed to open serial port" << std::endl;
return false;
}
struct termios options;
tcgetattr(fd_, &options);
cfsetispeed(&options, 100000);
cfsetospeed(&options, 100000);
options.c_cflag |= (CLOCAL | CREAD | CS8 | CSTOPB | PARENB);
options.c_cflag &= ~PARODD;
options.c_iflag |= (IGNPAR | PARMRK);
options.c_iflag &= ~(IXON | IXOFF | IXANY);
options.c_oflag &= ~OPOST;
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 0;
tcsetattr(fd_, TCSANOW, &options);
return true;
}
bool SbusRx::Read() {
bool new_data = false;
while (true) {
ssize_t bytes_read = read(fd_, &buf_[0], sizeof(buf_));
if (bytes_read <= 0) {
break;
}
if (Parse()) {
new_data = true;
}
}
return new_data;
}
bool SbusRx::Parse() {
static auto sbusTime = std::chrono::steady_clock::now();
static uint8_t parserState = 0;
static uint8_t prevByte = 0;
for (size_t i = 0; i < sizeof(buf_); ++i) {
auto currentTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::microseconds>(currentTime - sbusTime);
if (elapsedTime.count() > SBUS_TIMEOUT_US) {
parserState = 0;
}
sbusTime = currentTime;
uint8_t curByte = buf_[i];
if (parserState == 0) {
if (curByte == SBUS_HEADER && (prevByte == SBUS_FOOTER || (prevByte & SBUS2_MASK) == SBUS2_FOOTER)) {
parserState++;
}
} else {
if (parserState < PAYLOAD_SIZE) {
buf_[parserState++] = curByte;
}
if (parserState == PAYLOAD_SIZE) {
if (curByte == SBUS_FOOTER || (curByte & SBUS2_MASK) == SBUS2_FOOTER) {
// Extract channel data
data_.ch[0] = ((buf_[1] | buf_[2] << 8) & 0x07FF) - 1023;
data_.ch[1] = ((buf_[2] >> 3 | buf_[3] << 5) & 0x07FF) - 1023;
data_.ch[2] = ((buf_[3] >> 6 | buf_[4] << 2 | buf_[5] << 10) & 0x07FF) - 1023;
data_.ch[3] = ((buf_[5] >> 1 | buf_[6] << 7) & 0x07FF) - 1023;
data_.ch[4] = ((buf_[6] >> 4 | buf_[7] << 4) & 0x07FF) - 1023;
data_.ch[5] = ((buf_[7] >> 7 | buf_[8] << 1 | buf_[9] << 9) & 0x07FF) - 1023;
data_.ch[6] = ((buf_[9] >> 2 | buf_[10] << 6) & 0x07FF) - 1023;
data_.ch[7] = ((buf_[10] >> 5 | buf_[11] << 3) & 0x07FF) - 1023;
data_.ch[8] = ((buf_[12] | buf_[13] << 8) & 0x07FF) - 1023;
data_.ch[9] = ((buf_[13] >> 3 | buf_[14] << 5) & 0x07FF) - 1023;
data_.ch[10] = ((buf_[14] >> 6 | buf_[15] << 2 | buf_[16] << 10) & 0x07FF) - 1023;
data_.ch[11] = ((buf_[16] >> 1 | buf_[17] << 7) & 0x07FF) - 1023;
data_.ch[12] = ((buf_[17] >> 4 | buf_[18] << 4) & 0x07FF) - 1023;
data_.ch[13] = ((buf_[18] >> 7 | buf_[19] << 1 | buf_[20] << 9) & 0x07FF) - 1023;
data_.ch[14] = ((buf_[20] >> 2 | buf_[21] << 6) & 0x07FF) - 1023;
data_.ch[15] = ((buf_[21] >> 5 | buf_[22] << 3) & 0x07FF) - 1023;
// Grab the lost frame
data_.lostFrame = buf_[23] & LOST_FRAME_MASK;
// Grab the failsafe
data_.failsafe = buf_[23] & FAILSAFE_MASK;
// Grab the channel 17 and 18 values
data_.ch17 = buf_[23] & CH17_MASK;
data_.ch18 = buf_[23] & CH18_MASK;
parserState = 0;
return true;
} else {
parserState = 0;
}
}
}
prevByte = curByte;
}
return false;
}
} // namespace bfs
int main() {
bfs::SbusRx sbus("/dev/ttyS0");
if (!sbus.Begin()) {
std::cerr << "Failed to initialize SBUS receiver" << std::endl;
return 1;
}
while (true) {
if (sbus.Read()) {
const auto& data = sbus.data();
std::cout << "Channel Values:" << std::endl;
for (int i = 0; i < 16; ++i) {
std::cout << "Channel " << i + 1 << ": " << data.ch[i] << std::endl;
}
std::cout << "Channel 17: " << static_cast<int>(data.ch17) << std::endl;
std::cout << "Channel 18: " << static_cast<int>(data.ch18) << std::endl;
std::cout << "Lost Frame: " << data.lostFrame << std::endl;
std::cout << "Failsafe: " << data.failsafe << std::endl;
std::cout << std::endl;
}
usleep(10000);
}
return 0;
}
The output when sweeping channel 3 from 100 to -100.
Are there examples available for how to read the RC channels in either Python or C++ on the Navigator? Are there any special options required? When I was trying it in BlueOS/in Pirate mode looking at the individual variables, I wasn’t getting any RC readings either. I’m using the AT9S Pro and I have the receiver set to SBUS mode.