fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-27 20:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

protocol_splitter: simplify write()

Browse Source 
With the protocol header we don't need to parse the protocol anymore.
This commit is contained in:
Beat Küng 2021-08-13 14:10:48 +02:00 committed by Daniel Agar
parent 868f9cebb0
commit 6edb55c874
1 changed files with 26 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -283,13 +283,6 @@ protected:
Sp2Header _header;
uint16_t _packet_len;
enum class ParserState : uint8_t {
Idle = 0,
GotLength
};
ParserState _parser_state = ParserState::Idle;
ReadBuffer *_read_buffer;
};
@ -640,72 +633,24 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen)
{
/*
* we need to look into the data to make sure the output is locked for the duration
* of a whole packet.
* assumptions:
* - packet header is written all at once (or at least it contains the payload length)
* - a single write call does not contain multiple (or parts of multiple) packets
*/
ssize_t ret = 0;
_header.fields.len_h = (buflen >> 8) & 0x7f;
_header.fields.len_l = buflen & 0xff;
_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
lock(Write);
int buf_free = 0;
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
ssize_t ret = -1;
switch (_parser_state) {
case ParserState::Idle:
ASSERT(buflen >= 3);
if ((size_t)buf_free >= sizeof(_header) + buflen) {
ret = ::write(_fd, _header.bytes, sizeof(_header));
if ((unsigned char)buffer[0] == 253) {
uint8_t payload_len = buffer[1];
uint8_t incompat_flags = buffer[2];
_packet_len = payload_len + 12;
if (incompat_flags & 0x1) { //signing
_packet_len += 13;
}
_parser_state = ParserState::GotLength;
lock(Write);
} else if ((unsigned char)buffer[0] == 254) { // mavlink 1
uint8_t payload_len = buffer[1];
_packet_len = payload_len + 8;
_parser_state = ParserState::GotLength;
lock(Write);
} else {
PX4_ERR("parser error");
return 0;
if (ret == sizeof(_header)) {
ret = ::write(_fd, buffer, buflen);
}
/* FALLTHROUGH */
case ParserState::GotLength: {
_packet_len -= buflen;
int buf_free;
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
if (buf_free < (int)buflen) {
//let write fail, to let mavlink know the buffer would overflow
//(this is because in the ioctl we pretend there is always enough space)
ret = -1;
} else {
_header.fields.len_h = (buflen >> 8) & 0x7f;
_header.fields.len_l = buflen & 0xff;
_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
::write(_fd, _header.bytes, 4);
ret = ::write(_fd, buffer, buflen);
}
if (_packet_len == 0) {
unlock(Write);
_parser_state = ParserState::Idle;
}
}
break;
}
unlock(Write);
return ret;
}
@ -769,60 +714,24 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen)
{
/*
* we need to look into the data to make sure the output is locked for the duration
* of a whole packet.
* assumptions:
* - packet header is written all at once (or at least it contains the payload length)
* - a single write call does not contain multiple (or parts of multiple) packets
*/
ssize_t ret = 0;
uint16_t payload_len;
_header.fields.len_h = (buflen >> 8) & 0x7f;
_header.fields.len_l = buflen & 0xff;
_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
lock(Write);
int buf_free = 0;
ssize_t ret = -1;
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
switch (_parser_state) {
case ParserState::Idle:
ASSERT(buflen >= HEADER_SIZE);
if ((size_t)buf_free >= sizeof(_header) + buflen) {
ret = ::write(_fd, _header.bytes, sizeof(_header));
if (memcmp(buffer, ">>>", 3) != 0) {
PX4_ERR("parser error");
return 0;
if (ret == sizeof(_header)) {
ret = ::write(_fd, buffer, buflen);
}
payload_len = ((uint16_t)buffer[6] << 8) | buffer[7];
_packet_len = payload_len + HEADER_SIZE;
_parser_state = ParserState::GotLength;
lock(Write);
/* FALLTHROUGH */
case ParserState::GotLength: {
_packet_len -= buflen;
int buf_free;
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free);
// TODO should I care about this for rtps?
if ((unsigned)buf_free < buflen) {
//let write fail, to let rtps know the buffer would overflow
//(this is because in the ioctl we pretend there is always enough space)
ret = -1;
} else {
_header.fields.len_h = (buflen >> 8) & 0x7f;
_header.fields.len_l = buflen & 0xff;
_header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2];
::write(_fd, _header.bytes, 4);
ret = ::write(_fd, buffer, buflen);
}
if (_packet_len == 0) {
unlock(Write);
_parser_state = ParserState::Idle;
}
}
break;
}
unlock(Write);
return ret;
}