#include <iostream> #include <asm/termios.h> #include <asm/ioctls.h> #include <sys/signal.h> #include <stropts.h> #include <unistd.h> using namespace std; bool Ports::setup_port_Sbus(Info* port) { //> check descriptor if (!isatty(fd)) { throw "File description is NOT a serial port!"; } //> read fole descriptor config struct termios2 config = { 0 }; //> for interrupt struct sigaction saio; sigset_t mskvar; sigfillset(&mskvar); sigprocmask(SIG_SETMASK, &mskvar, NULL); sigdelset(&mskvar, SIGIO); saio.sa_handler = signal_io_handler; saio.sa_flags = 0; saio.sa_restorer = NULL; sigaction(SIGIO, &saio, NULL); if (ioctl(fd, TCGETS2, &config) < 0) { throw "Could not read config of FD!"; } config.c_cflag |= PARENB; // enable parity config.c_cflag &= ~PARODD; // even parity config.c_cflag |= CSTOPB; // enable 2 stop bits config.c_cflag &= ~CSIZE; // clear character size mask config.c_cflag |= CS8; // 8 bit characters config.c_cflag &= ~CRTSCTS; // disable hardware flow control config.c_cflag |= CREAD; // enable receiver config.c_cflag |= CLOCAL; // ignore modem lines config.c_lflag &= ~ICANON; // receive characters as they come in config.c_lflag &= ~ECHO; // do not echo config.c_lflag &= ~ISIG; // do not generate signals config.c_lflag &= ~IEXTEN; // disable implementation-defined processing config.c_iflag &= ~(IXON | IXOFF | IXANY); // disable XON/XOFF flow control config.c_iflag |= IGNBRK; // ignore BREAK condition config.c_iflag |= INPCK; // enable parity checking config.c_iflag |= IGNPAR; // ignore framing and parity errors config.c_iflag &= ~ISTRIP; // do not strip off 8th bit config.c_iflag &= ~INLCR; // do not translate NL to CR config.c_iflag &= ~ICRNL; // do not translate CR to NL config.c_iflag &= ~IGNCR; // do not ignore CR config.c_oflag &= ~OPOST; // disable implementation-defined processing config.c_oflag &= ~ONLCR; // do not map NL to CR-NL config.c_oflag &= ~OCRNL; // do not map CR to NL config.c_oflag &= ~(ONOCR | ONLRET); // output CR like a normal person config.c_oflag &= ~OFILL; // no fill characters // Apply baudrate speed_t br = 100000; config.c_ispeed = br; config.c_ospeed = br; config.c_cc[VMIN] = 0; config.c_cc[VTIME] = 0; //> Finally, apply the configuration if (ioctl(fd, TCSETS2, &config) < 0) { throw "Could not set configuration of fd!"; } // Done! return true; } void port_init(){ const char* _name = "/dev/ttyTHS1"; int fd = -1; fd = open(_name, O_RDWR | O_NOCTTY ); if (fd < 0) { throw "File is not open!"; } else{ fcntl(fd, F_SETFL, O_NONBLOCK|O_ASYNC); } bool setup = setup_port_Sbus(fd); if (!setup) { throw "Could not configure port"; } if (fd <= 0) { throw "Connection attempt to port failed, exiting"; } } struct Sbus_data{ bool lost_frame; int failsafe; bool ch17; bool ch18; static int8_t SBUS_NUM_CH = 16; int last_ch[SBUS_NUM_CH] = {}; int ch[SBUS_NUM_CH] = {}; }; int main(){ try{ port_init(); } catch(const char* err){ cout << "ERR: " << err << endl; } uint8_t byte; uint8_t sb_buf[SBUS_PACKET_LEN] = {}; const int8_t SBUS_PACKET_LEN = 25; const int8_t SBUS_NUM_SBUS_CH = 16; const uint8_t SBUS_HEADER = 0x0F; const uint8_t SBUS_FOOTER = 0x00; const uint8_t SBUS_FOOTER2 = 0x04; const uint8_t SBUS_CH17_MASK = 0x01; const uint8_t SBUS_CH18_MASK = 0x02; const uint8_t SBUS_LOST_FRAME_MASK = 0x04; const uint8_t SBUS_FAILSAFE_MASK = 0x08; Sbus_data sb_data; int sb_state = 0; do{ count_read_symb = read(port->file_descriptor, &byte, 1); if(count_read_symb <= 0) { cout << "byte: " << byte << " | errno: "<< errno << endl; continue; } //> wrong start if(sb_state == 0 && byte != SBUS_HEADER) { continue; } sb_buf[sb_state++] = byte; //> index done if(sb_state == SBUS_PACKET_LEN) { sb_state = 0; if(sb_buf[24] != SBUS_FOOTER) { continue; } sb_data.ch[0] = ((sb_buf[1] |sb_buf[2]<<8) & 0x07FF); sb_data.ch[1] = ((sb_buf[2]>>3 |sb_buf[3]<<5) & 0x07FF); sb_data.ch[2] = ((sb_buf[3]>>6 |sb_buf[4]<<2 |sb_buf[5]<<10) & 0x07FF); sb_data.ch[3] = ((sb_buf[5]>>1 |sb_buf[6]<<7) & 0x07FF); sb_data.ch[4] = ((sb_buf[6]>>4 |sb_buf[7]<<4) & 0x07FF); sb_data.ch[5] = ((sb_buf[7]>>7 |sb_buf[8]<<1 |sb_buf[9]<<9) & 0x07FF); sb_data.ch[6] = ((sb_buf[9]>>2 |sb_buf[10]<<6) & 0x07FF); sb_data.ch[7] = ((sb_buf[10]>>5|sb_buf[11]<<3) & 0x07FF); sb_data.ch[8] = ((sb_buf[12] |sb_buf[13]<<8) & 0x07FF); sb_data.ch[9] = ((sb_buf[13]>>3|sb_buf[14]<<5) & 0x07FF); sb_data.ch[10] = ((sb_buf[14]>>6|sb_buf[15]<<2|sb_buf[16]<<10) & 0x07FF); sb_data.ch[11] = ((sb_buf[16]>>1|sb_buf[17]<<7) & 0x07FF); sb_data.ch[12] = ((sb_buf[17]>>4|sb_buf[18]<<4) & 0x07FF); sb_data.ch[13] = ((sb_buf[18]>>7|sb_buf[19]<<1|sb_buf[20]<<9) & 0x07FF); sb_data.ch[14] = ((sb_buf[20]>>2|sb_buf[21]<<6) & 0x07FF); sb_data.ch[15] = ((sb_buf[21]>>5|sb_buf[22]<<3) & 0x07FF); ((sb_buf[23]) & 0x0001) ? sb_data.ch[16] = 2047: sb_data.ch[16] = 0; ((sb_buf[23] >> 1) & 0x0001) ? sb_data.ch[17] = 2047: sb_data.ch[17] = 0; if ((sb_buf[23] >> 3) & 0x0001) { sb_data.failsafe = 1; } else { sb_data.failsafe = 0; } cout << "DONE!" << endl; } } while(count_read_symb > 0); }I checked ports buffer with 