fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 01:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

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