Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

protocol_splitter: Sp2Header defined as union (#17511)

Browse Source 
Aligned with agent_protocol_splitter to make byte access easier
for checksum generation

Co-authored-by: Nuno Marques <n.marques21@hotmail.com>
This commit is contained in:
Jari Nippula
2021-05-04 11:09:24 +03:00
committed by GitHub
parent 4e69952ee4
commit 04b7ee43bc
1 changed files with 37 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/protocol_splitter/protocol_splitter.cpp
+37 -34
View File
@@ -76,13 +76,16 @@ header[1] - |T| LenH |
header[2] - | LenL | header[2] - | LenL |
header[3] - | Checksum | header[3] - | Checksum |
*/ */
typedef struct __attribute__((packed)) typedef union __attribute__((packed))
{ {
char magic; // 'S' uint8_t bytes[4];
uint8_t len_h: 7, // Length MSB struct {
type: 1; // 0=MAVLINK, 1=RTPS char magic; // 'S'
uint8_t len_l; // Length LSB uint8_t len_h: 7, // Length MSB
uint8_t checksum; // XOR of two above bytes type: 1; // 0=MAVLINK, 1=RTPS
uint8_t len_l; // Length LSB
uint8_t checksum; // XOR of two above bytes
} fields;
} Sp2Header_t; } Sp2Header_t;
struct StaticData { struct StaticData {
@@ -288,11 +291,11 @@ Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer)
: DevCommon("/dev/mavlink") : DevCommon("/dev/mavlink")
, _read_buffer{read_buffer} , _read_buffer{read_buffer}
{ {
_header.magic = Sp2HeaderMagic; _header.fields.magic = Sp2HeaderMagic;
_header.len_h = 0; _header.fields.len_h = 0;
_header.len_l = 0; _header.fields.len_l = 0;
_header.checksum = 0; _header.fields.checksum = 0;
_header.type = MessageType::Mavlink; _header.fields.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)
@@ -342,9 +345,10 @@ 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) &&
(((Sp2Header_t *) &_read_buffer->buffer[i])->magic != Sp2HeaderMagic (((Sp2Header_t *) &_read_buffer->buffer[i])->fields.magic != Sp2HeaderMagic
|| ((Sp2Header_t *) &_read_buffer->buffer[i])->type != (uint8_t) MessageType::Mavlink || ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.type != (uint8_t)MessageType::Mavlink
|| ((Sp2Header_t *) &_read_buffer->buffer[i])->checksum != (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]) || ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.checksum !=
(_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2])
)) { )) {
i++; i++;
} }
@@ -355,7 +359,7 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
} }
header = (Sp2Header_t *)&_read_buffer->buffer[i]; header = (Sp2Header_t *)&_read_buffer->buffer[i];
payload_len = ((uint16_t)header->len_h << 8) | header->len_l; payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.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
@@ -436,11 +440,10 @@ ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen)
ret = -1; ret = -1;
} else { } else {
uint8_t *bytes = (uint8_t *) &_header; _header.fields.len_h = (buflen >> 8) & 0x7f;
_header.len_h = (buflen >> 8) & 0x7f; _header.fields.len_l = buflen & 0xff;
_header.len_l = buflen & 0xff; _header.fields.checksum = _header.bytes[1] ^ _header.bytes[2];
_header.checksum = bytes[1] ^ bytes[2]; ::write(_fd, _header.bytes, 4);
::write(_fd, bytes, 4);
ret = ::write(_fd, buffer, buflen); ret = ::write(_fd, buffer, buflen);
} }
@@ -475,11 +478,11 @@ RtpsDev::RtpsDev(ReadBuffer *read_buffer)
: DevCommon("/dev/rtps") : DevCommon("/dev/rtps")
, _read_buffer{read_buffer} , _read_buffer{read_buffer}
{ {
_header.magic = Sp2HeaderMagic; _header.fields.magic = Sp2HeaderMagic;
_header.len_h = 0; _header.fields.len_h = 0;
_header.len_l = 0; _header.fields.len_l = 0;
_header.checksum = 0; _header.fields.checksum = 0;
_header.type = MessageType::Rtps; _header.fields.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)
@@ -509,9 +512,10 @@ 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) &&
(((Sp2Header_t *) &_read_buffer->buffer[i])->magic != Sp2HeaderMagic (((Sp2Header_t *) &_read_buffer->buffer[i])->fields.magic != Sp2HeaderMagic
|| ((Sp2Header_t *) &_read_buffer->buffer[i])->type != (uint8_t) MessageType::Rtps || ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.type != (uint8_t)MessageType::Rtps
|| ((Sp2Header_t *) &_read_buffer->buffer[i])->checksum != (_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]) || ((Sp2Header_t *) &_read_buffer->buffer[i])->fields.checksum !=
(_read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2])
)) { )) {
i++; i++;
} }
@@ -522,7 +526,7 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
} }
header = (Sp2Header_t *)&_read_buffer->buffer[i]; header = (Sp2Header_t *)&_read_buffer->buffer[i];
payload_len = ((uint16_t)header->len_h << 8) | header->len_l; payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.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
@@ -585,11 +589,10 @@ ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen)
ret = -1; ret = -1;
} else { } else {
uint8_t *bytes = (uint8_t *) &_header; _header.fields.len_h = (buflen >> 8) & 0x7f;
_header.len_h = (buflen >> 8) & 0x7f; _header.fields.len_l = buflen & 0xff;
_header.len_l = buflen & 0xff; _header.fields.checksum = _header.bytes[1] ^ _header.bytes[2];
_header.checksum = bytes[1] ^ bytes[2]; ::write(_fd, _header.bytes, 4);
::write(_fd, bytes, 4);
ret = ::write(_fd, buffer, buflen); ret = ::write(_fd, buffer, buflen);
} }