Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 12:45:36 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Utilize header struct instead of control bytes directly

Browse Source
This commit is contained in:
Jari Nippula
2021-04-21 13:43:00 +03:00
committed by Nuno Marques
parent 757f1df068
commit 9980d2a556
1 changed files with 62 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/protocol_splitter/protocol_splitter.cpp
+62 -47
View File
@@ -55,10 +55,38 @@ class ReadBuffer;
extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]); 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 char Sp2HeaderMagic = 'S';
const int Sp2HeaderSize = 4; 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 { struct StaticData {
Mavlink2Dev *mavlink2; Mavlink2Dev *mavlink2;
RtpsDev *rtps; RtpsDev *rtps;
@@ -145,29 +173,7 @@ public:
protected: protected:
/* Sp2Header_t _header;
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] = {};
virtual pollevent_t poll_state(struct file *filp); virtual pollevent_t poll_state(struct file *filp);
@@ -284,16 +290,19 @@ Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer)
: DevCommon("/dev/mavlink") : DevCommon("/dev/mavlink")
, _read_buffer{read_buffer} , _read_buffer{read_buffer}
{ {
_header[0] = Sp2HeaderMagic; _header.magic = Sp2HeaderMagic;
_header[1] = 0; _header.len_h = 0;
_header[2] = 0; _header.len_l = 0;
_header[3] = 0; _header.checksum = 0;
_header.type = MessageType::Mavlink;
} }
ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
{ {
int i, ret; int i, ret;
uint16_t packet_len, payload_len; uint16_t packet_len, payload_len;
Sp2Header_t *header;
/* last reading was partial (i.e., buffer didn't fit whole message), /* last reading was partial (i.e., buffer didn't fit whole message),
* so now we'll just send remaining bytes */ * 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; i = 0;
while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) &&
(_read_buffer->buffer[i] != Sp2HeaderMagic (((Sp2Header_t *) &_read_buffer->buffer[i])->magic != Sp2HeaderMagic
|| (_read_buffer->buffer[i + 1] & 0x80) != (uint8_t) MessageType::Mavlink || ((Sp2Header_t *) &_read_buffer->buffer[i])->type != (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])->checksum != (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2])
)) { )) {
i++; i++;
} }
@@ -348,7 +357,8 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
goto end; 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_len = payload_len + Sp2HeaderSize;
// packet is bigger than what we've read, better luck next time // 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; ret = -1;
} else { } else {
_header[1] = (uint8_t) MessageType::Mavlink | (uint8_t)((buflen >> 8) & 0x7f); uint8_t *bytes = (uint8_t *) &_header;
_header[2] = (uint8_t)(buflen & 0xff); _header.len_h = (buflen >> 8) & 0x7f;
_header[3] = _header[1] ^ _header[2]; _header.len_l = buflen & 0xff;
::write(_fd, _header, 4); _header.checksum = bytes[1] ^ bytes[2];
::write(_fd, bytes, 4);
ret = ::write(_fd, buffer, buflen); ret = ::write(_fd, buffer, buflen);
} }
@@ -467,16 +478,18 @@ RtpsDev::RtpsDev(ReadBuffer *read_buffer)
: DevCommon("/dev/rtps") : DevCommon("/dev/rtps")
, _read_buffer{read_buffer} , _read_buffer{read_buffer}
{ {
_header[0] = Sp2HeaderMagic; _header.magic = Sp2HeaderMagic;
_header[1] = 0; _header.len_h = 0;
_header[2] = 0; _header.len_l = 0;
_header[3] = 0; _header.checksum = 0;
_header.type = MessageType::Rtps;
} }
ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
{ {
int i, ret; int i, ret;
uint16_t packet_len, payload_len; uint16_t packet_len, payload_len;
Sp2Header_t *header;
if (!_had_data) { if (!_had_data) {
return 0; return 0;
@@ -499,9 +512,9 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
i = 0; i = 0;
while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) &&
(_read_buffer->buffer[i] != Sp2HeaderMagic (((Sp2Header_t *) &_read_buffer->buffer[i])->magic != Sp2HeaderMagic
|| (_read_buffer->buffer[i + 1] & 0x80) != (uint8_t) MessageType::Rtps || ((Sp2Header_t *) &_read_buffer->buffer[i])->type != (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])->checksum != (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2])
)) { )) {
i++; i++;
} }
@@ -511,7 +524,8 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
goto end; 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_len = payload_len + Sp2HeaderSize;
// packet is bigger than what we've read, better luck next time // 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; ret = -1;
} else { } else {
_header[1] = MessageType::Rtps | (uint8_t)((buflen >> 8) & 0x7f); uint8_t *bytes = (uint8_t *) &_header;
_header[2] = (uint8_t)(buflen & 0xff); _header.len_h = (buflen >> 8) & 0x7f;
_header[3] = _header[1] ^ _header[2]; _header.len_l = buflen & 0xff;
::write(_fd, _header, 4); _header.checksum = bytes[1] ^ bytes[2];
::write(_fd, bytes, 4);
ret = ::write(_fd, buffer, buflen); ret = ::write(_fd, buffer, buflen);
} }