Fixed format issues and missing micro-CDR in a config file

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2017-07-26 22:55:56 -07:00
committed by Lorenz Meier
parent 2b86dd1fdb
commit b19dc0650e
3 changed files with 28 additions and 8 deletions
+3
View File
@@ -14,6 +14,9 @@ exec find src \
-path src/lib/DriverFramework -prune -o \ -path src/lib/DriverFramework -prune -o \
-path src/lib/ecl -prune -o \ -path src/lib/ecl -prune -o \
-path src/lib/matrix -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/commander -prune -o \
-path src/modules/sdlog2 -prune -o \ -path src/modules/sdlog2 -prune -o \
-path src/modules/systemlib/uthash -prune -o \ -path src/modules/systemlib/uthash -prune -o \
@@ -58,6 +58,7 @@ set(config_module_list
modules/systemlib/param modules/systemlib/param
modules/systemlib modules/systemlib
lib/version lib/version
lib/micro-CDR
# #
# Libraries # Libraries
@@ -94,8 +94,10 @@ int ReadBuffer::read(int fd)
} }
int r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size); int r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size);
if (r < 0)
if (r < 0) {
return r; return r;
}
buf_size += r; buf_size += r;
@@ -133,6 +135,7 @@ protected:
void lock(enum Operation op) void lock(enum Operation op)
{ {
sem_t *lock = op == Read ? &objects->r_lock : &objects->w_lock; sem_t *lock = op == Read ? &objects->r_lock : &objects->w_lock;
while (sem_wait(lock) != 0) { while (sem_wait(lock) != 0) {
/* The only case that an error should occur here is if /* The only case that an error should occur here is if
* the wait was awakened by a signal. * 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 */ * so now we'll just send remaining bytes */
if (_remaining_partial > 0) { if (_remaining_partial > 0) {
size_t len = _remaining_partial; size_t len = _remaining_partial;
if (buflen < len) { if (buflen < len) {
len = buflen; len = buflen;
} }
memmove(buffer, _partial_buffer + _partial_start, len); memmove(buffer, _partial_buffer + _partial_start, len);
_partial_start += len; _partial_start += len;
_remaining_partial -= len; _remaining_partial -= len;
@@ -262,6 +267,7 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
if (_remaining_partial == 0) { if (_remaining_partial == 0) {
_partial_start = 0; _partial_start = 0;
} }
return len; return len;
} }
@@ -272,20 +278,24 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen)
lock(Read); lock(Read);
ret = _read_buffer->read(_fd); ret = _read_buffer->read(_fd);
if (ret < 0) if (ret < 0) {
goto end; goto end;
}
ret = 0; ret = 0;
if (_read_buffer->buf_size < 3) if (_read_buffer->buf_size < 3) {
goto end; goto end;
}
// Search for a mavlink packet on buffer to send it // Search for a mavlink packet on buffer to send it
i = 0; i = 0;
while (i < (_read_buffer->buf_size - 3) while (i < (_read_buffer->buf_size - 3)
&& _read_buffer->buffer[i] != 253 && _read_buffer->buffer[i] != 253
&& _read_buffer->buffer[i] != 254) && _read_buffer->buffer[i] != 254) {
i++; i++;
}
// We need at least the first three bytes to get packet len // We need at least the first three bytes to get packet len
if (i >= _read_buffer->buf_size - 3) { 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 if (incompat_flags & 0x1) { //signing
packet_len += 13; packet_len += 13;
} }
} else { } else {
packet_len = _read_buffer->buffer[i + 1] + 8; 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) { if (ret < 0) {
goto end; goto end;
} }
ret = 0; ret = 0;
if (_read_buffer->buf_size < HEADER_SIZE) if (_read_buffer->buf_size < HEADER_SIZE) {
goto end; // starting ">>>" + topic + seq + lenhigh + lenlow + crchigh + crclow goto end; // starting ">>>" + topic + seq + lenhigh + lenlow + crchigh + crclow
}
// Search for a rtps packet on buffer to send it // Search for a rtps packet on buffer to send it
i = 0; 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++; i++;
}
// We need at least the first six bytes to get packet len // We need at least the first six bytes to get packet len
if (i >= _read_buffer->buf_size - HEADER_SIZE) { 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) { switch (_parser_state) {
case ParserState::Idle: case ParserState::Idle:
ASSERT(buflen >= HEADER_SIZE); ASSERT(buflen >= HEADER_SIZE);
if (memcmp(buffer, ">>>", 3) != 0) { if (memcmp(buffer, ">>>", 3) != 0) {
PX4_ERR("parser error"); PX4_ERR("parser error");
return 0; return 0;