mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 18:47:21 +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/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;
|
||||||
|
|||||||
Reference in New Issue
Block a user