mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 04:33:10 +08:00
Fixed format issues and missing micro-CDR in a config file
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
committed by
Lorenz Meier
parent
2b86dd1fdb
commit
b19dc0650e
@@ -14,6 +14,9 @@ exec find src \
|
||||
-path src/lib/DriverFramework -prune -o \
|
||||
-path src/lib/ecl -prune -o \
|
||||
-path src/lib/matrix -prune -o \
|
||||
-path src/lib/micro-CDR -prune -o \
|
||||
-path src/modules/micrortps_bridge/build -prune -o \
|
||||
-path src/modules/micrortps_bridge/micrortps_agent -prune -o \
|
||||
-path src/modules/commander -prune -o \
|
||||
-path src/modules/sdlog2 -prune -o \
|
||||
-path src/modules/systemlib/uthash -prune -o \
|
||||
|
||||
@@ -58,6 +58,7 @@ set(config_module_list
|
||||
modules/systemlib/param
|
||||
modules/systemlib
|
||||
lib/version
|
||||
lib/micro-CDR
|
||||
|
||||
#
|
||||
# Libraries
|
||||
|
||||
@@ -94,8 +94,10 @@ int ReadBuffer::read(int fd)
|
||||
}
|
||||
|
||||
int r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size);
|
||||
if (r < 0)
|
||||
|
||||
if (r < 0) {
|
||||
return r;
|
||||
}
|
||||
|
||||
buf_size += r;
|
||||
|
||||
@@ -133,6 +135,7 @@ protected:
|
||||
void lock(enum Operation op)
|
||||
{
|
||||
sem_t *lock = op == Read ? &objects->r_lock : &objects->w_lock;
|
||||
|
||||
while (sem_wait(lock) != 0) {
|
||||
/* The only case that an error should occur here is if
|
||||
* the wait was awakened by a signal.
|
||||
@@ -252,9 +255,11 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* so now we'll just send remaining bytes */
|
||||
if (_remaining_partial > 0) {
|
||||
size_t len = _remaining_partial;
|
||||
|
||||
if (buflen < len) {
|
||||
len = buflen;
|
||||
}
|
||||
|
||||
memmove(buffer, _partial_buffer + _partial_start, len);
|
||||
_partial_start += len;
|
||||
_remaining_partial -= len;
|
||||
@@ -262,6 +267,7 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
|
||||
if (_remaining_partial == 0) {
|
||||
_partial_start = 0;
|
||||
}
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
@@ -272,20 +278,24 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
|
||||
lock(Read);
|
||||
ret = _read_buffer->read(_fd);
|
||||
|
||||
if (ret < 0)
|
||||
if (ret < 0) {
|
||||
goto end;
|
||||
}
|
||||
|
||||
ret = 0;
|
||||
|
||||
if (_read_buffer->buf_size < 3)
|
||||
if (_read_buffer->buf_size < 3) {
|
||||
goto end;
|
||||
}
|
||||
|
||||
// Search for a mavlink packet on buffer to send it
|
||||
i = 0;
|
||||
|
||||
while (i < (_read_buffer->buf_size - 3)
|
||||
&& _read_buffer->buffer[i] != 253
|
||||
&& _read_buffer->buffer[i] != 254)
|
||||
&& _read_buffer->buffer[i] != 253
|
||||
&& _read_buffer->buffer[i] != 254) {
|
||||
i++;
|
||||
}
|
||||
|
||||
// We need at least the first three bytes to get packet len
|
||||
if (i >= _read_buffer->buf_size - 3) {
|
||||
@@ -300,6 +310,7 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
|
||||
if (incompat_flags & 0x1) { //signing
|
||||
packet_len += 13;
|
||||
}
|
||||
|
||||
} else {
|
||||
packet_len = _read_buffer->buffer[i + 1] + 8;
|
||||
}
|
||||
@@ -429,15 +440,19 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen)
|
||||
if (ret < 0) {
|
||||
goto end;
|
||||
}
|
||||
|
||||
ret = 0;
|
||||
|
||||
if (_read_buffer->buf_size < HEADER_SIZE)
|
||||
goto end; // starting ">>>" + topic + seq + lenhigh + lenlow + crchigh + crclow
|
||||
if (_read_buffer->buf_size < HEADER_SIZE) {
|
||||
goto end; // starting ">>>" + topic + seq + lenhigh + lenlow + crchigh + crclow
|
||||
}
|
||||
|
||||
// Search for a rtps packet on buffer to send it
|
||||
i = 0;
|
||||
while (i < (_read_buffer->buf_size - HEADER_SIZE) && (memcmp(_read_buffer->buffer + i, ">>>", 3) != 0))
|
||||
|
||||
while (i < (_read_buffer->buf_size - HEADER_SIZE) && (memcmp(_read_buffer->buffer + i, ">>>", 3) != 0)) {
|
||||
i++;
|
||||
}
|
||||
|
||||
// We need at least the first six bytes to get packet len
|
||||
if (i >= _read_buffer->buf_size - HEADER_SIZE) {
|
||||
@@ -481,6 +496,7 @@ ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen)
|
||||
switch (_parser_state) {
|
||||
case ParserState::Idle:
|
||||
ASSERT(buflen >= HEADER_SIZE);
|
||||
|
||||
if (memcmp(buffer, ">>>", 3) != 0) {
|
||||
PX4_ERR("parser error");
|
||||
return 0;
|
||||
|
||||
Reference in New Issue
Block a user