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

Merge branch 'sf0x_paranoid' of github.com:PX4/Firmware into swissfang

This commit is contained in:
Lorenz Meier 2014-08-31 19:54:03 +02:00
parent b81277a1ef ee91320299
commit 7109d3b32e
1 changed files with 16 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/drivers/sf0x/sf0x.cpp
View File

@ -520,11 +520,9 @@ SF0X::collect()
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* timed out - retry */
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
_linebuf_index = 0;
} else if (_linebuf_index > 0) {
/* increment to next read position */
_linebuf_index++;
}
/* the buffer for read chars is buflen minus null termination */
@ -550,18 +548,19 @@ SF0X::collect()
return -EAGAIN;
}
/* we did increment the index to the next position already, so just add the additional fields */
_linebuf_index += (ret - 1);
/* let the write pointer point to the next free entry */
_linebuf_index += ret;
_last_read = hrt_absolute_time();
if (_linebuf_index < 1) {
/* we need at least the two end bytes to make sense of this string */
/* require a reasonable amount of minimum bytes */
if (_linebuf_index < 6) {
/* we need at this format: x.xx\r\n */
return -EAGAIN;
} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
} else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
if (_linebuf_index >= readlen - 1) {
if (_linebuf_index == readlen) {
/* we have a full buffer, but no line ending - abort */
_linebuf_index = 0;
perf_count(_comms_errors);
@ -577,9 +576,7 @@ SF0X::collect()
bool valid;
/* enforce line ending */
unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
_linebuf[lend] = '\0';
_linebuf[_linebuf_index] = '\0';
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
@ -591,17 +588,12 @@ SF0X::collect()
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (unsigned i = 0; i < (lend - 2); i++) {
for (unsigned i = 0; i < (_linebuf_index - 2); i++) {
if (_linebuf[i] == '\n') {
/* allocate temporary buffer */
char buf[sizeof(_linebuf)];
/* copy remainder of buffer (2nd measurement) to temporary buffer */
memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
/* copy temporary buffer to beginning of line buffer,
* effectively overwriting a previous temporary
* measurement
*/
memcpy(_linebuf, buf, (lend + 1) - (i + 1));
/* wipe out any partial measurements */
for (unsigned j = 0; j <= i; j++) {
_linebuf[j] = ' ';
}
}
/* we need a digit before the dot and a dot for a valid number */
@ -613,8 +605,8 @@ SF0X::collect()
if (valid) {
si_units = strtod(_linebuf, &end);
/* we require at least 3 characters for a valid number */
if (end > _linebuf + 3) {
/* we require at least four characters for a valid number */
if (end > _linebuf + 4) {
valid = true;
} else {
si_units = -1.0f;