dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Utilize header struct instead of control bytes directly

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
src/drivers/protocol_splitter/protocol_splitter.cpp
View File

@ -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);
}