diff --git a/src/drivers/protocol_splitter/protocol_splitter.cpp b/src/drivers/protocol_splitter/protocol_splitter.cpp index 7f174a1a11..28479bda28 100644 --- a/src/drivers/protocol_splitter/protocol_splitter.cpp +++ b/src/drivers/protocol_splitter/protocol_splitter.cpp @@ -55,10 +55,38 @@ class ReadBuffer; extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]); +/* + + +MessageType is in MSB of header[1] + | + v + Mavlink 0000 0000b + Rtps 1000 0000b +*/ +enum MessageType {Mavlink = 0x00, Rtps = 0x01}; const char Sp2HeaderMagic = 'S'; const int Sp2HeaderSize = 4; +/* +Header Structure: + + bits: 1 2 3 4 5 6 7 8 +header[0] - | Magic | +header[1] - |T| LenH | +header[2] - | LenL | +header[3] - | Checksum | +*/ +typedef struct __attribute__((packed)) +{ + char magic; // 'S' + uint8_t len_h: 7, // Length MSB + type: 1; // 0=MAVLINK, 1=RTPS + uint8_t len_l; // Length LSB + uint8_t checksum; // XOR of two above bytes +} Sp2Header_t; + struct StaticData { Mavlink2Dev *mavlink2; RtpsDev *rtps; @@ -145,29 +173,7 @@ public: protected: - /* - struct Sp2Header { - char magic; // 'S' - uint8_t type:1; // 0=MAVLINK, 1=RTPS - uint16_t payload_len:15; // Length - uint8_t checksum; // XOR of two above bytes - } - - bits: 1 2 3 4 5 6 7 8 - header[0] - | Magic | - header[1] - |T| LenH | - header[2] - | LenL | - header[3] - | Checksum | - - MessageType is in MSB of header[1] - | - v - Mavlink 0000 0000b - Rtps 1000 0000b - */ - enum MessageType {Mavlink = 0x00, Rtps = 0x80}; - - uint8_t _header[4] = {}; + Sp2Header_t _header; virtual pollevent_t poll_state(struct file *filp); @@ -284,16 +290,19 @@ Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer) : DevCommon("/dev/mavlink") , _read_buffer{read_buffer} { - _header[0] = Sp2HeaderMagic; - _header[1] = 0; - _header[2] = 0; - _header[3] = 0; + _header.magic = Sp2HeaderMagic; + _header.len_h = 0; + _header.len_l = 0; + _header.checksum = 0; + _header.type = MessageType::Mavlink; + } ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) { int i, ret; uint16_t packet_len, payload_len; + Sp2Header_t *header; /* last reading was partial (i.e., buffer didn't fit whole message), * so now we'll just send remaining bytes */ @@ -336,9 +345,9 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) i = 0; while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && - (_read_buffer->buffer[i] != Sp2HeaderMagic - || (_read_buffer->buffer[i + 1] & 0x80) != (uint8_t) MessageType::Mavlink - || (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]) != _read_buffer->buffer[i + 3] + (((Sp2Header_t *) &_read_buffer->buffer[i])->magic != Sp2HeaderMagic + || ((Sp2Header_t *) &_read_buffer->buffer[i])->type != (uint8_t) MessageType::Mavlink + || ((Sp2Header_t *) &_read_buffer->buffer[i])->checksum != (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]) )) { i++; } @@ -348,7 +357,8 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) goto end; } - payload_len = ((uint16_t)(_read_buffer->buffer[i + 1] & 0x7f) << 8) | _read_buffer->buffer[i + 2]; + header = (Sp2Header_t *)&_read_buffer->buffer[i]; + payload_len = ((uint16_t)header->len_h << 8) | header->len_l; packet_len = payload_len + Sp2HeaderSize; // packet is bigger than what we've read, better luck next time @@ -429,10 +439,11 @@ ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen) ret = -1; } else { - _header[1] = (uint8_t) MessageType::Mavlink | (uint8_t)((buflen >> 8) & 0x7f); - _header[2] = (uint8_t)(buflen & 0xff); - _header[3] = _header[1] ^ _header[2]; - ::write(_fd, _header, 4); + uint8_t *bytes = (uint8_t *) &_header; + _header.len_h = (buflen >> 8) & 0x7f; + _header.len_l = buflen & 0xff; + _header.checksum = bytes[1] ^ bytes[2]; + ::write(_fd, bytes, 4); ret = ::write(_fd, buffer, buflen); } @@ -467,16 +478,18 @@ RtpsDev::RtpsDev(ReadBuffer *read_buffer) : DevCommon("/dev/rtps") , _read_buffer{read_buffer} { - _header[0] = Sp2HeaderMagic; - _header[1] = 0; - _header[2] = 0; - _header[3] = 0; + _header.magic = Sp2HeaderMagic; + _header.len_h = 0; + _header.len_l = 0; + _header.checksum = 0; + _header.type = MessageType::Rtps; } ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) { int i, ret; uint16_t packet_len, payload_len; + Sp2Header_t *header; if (!_had_data) { return 0; @@ -499,9 +512,9 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) i = 0; while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && - (_read_buffer->buffer[i] != Sp2HeaderMagic - || (_read_buffer->buffer[i + 1] & 0x80) != (uint8_t) MessageType::Rtps - || (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]) != _read_buffer->buffer[i + 3] + (((Sp2Header_t *) &_read_buffer->buffer[i])->magic != Sp2HeaderMagic + || ((Sp2Header_t *) &_read_buffer->buffer[i])->type != (uint8_t) MessageType::Rtps + || ((Sp2Header_t *) &_read_buffer->buffer[i])->checksum != (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]) )) { i++; } @@ -511,7 +524,8 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) goto end; } - payload_len = ((uint16_t)(_read_buffer->buffer[i + 1] & 0x7f) << 8) | _read_buffer->buffer[i + 2]; + header = (Sp2Header_t *)&_read_buffer->buffer[i]; + payload_len = ((uint16_t)header->len_h << 8) | header->len_l; packet_len = payload_len + Sp2HeaderSize; // packet is bigger than what we've read, better luck next time @@ -574,10 +588,11 @@ ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen) ret = -1; } else { - _header[1] = MessageType::Rtps | (uint8_t)((buflen >> 8) & 0x7f); - _header[2] = (uint8_t)(buflen & 0xff); - _header[3] = _header[1] ^ _header[2]; - ::write(_fd, _header, 4); + uint8_t *bytes = (uint8_t *) &_header; + _header.len_h = (buflen >> 8) & 0x7f; + _header.len_l = buflen & 0xff; + _header.checksum = bytes[1] ^ bytes[2]; + ::write(_fd, bytes, 4); ret = ::write(_fd, buffer, buflen); }