diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-31 21:10:31 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-31 21:10:31 +0200 |
commit | ec740c5509632366e8e4a87ac9ae5731b6fb727c (patch) | |
tree | 357fdec3a16ad43fe485a6ca63ea698bf00b5e42 /src | |
parent | f123e3ae1ae3ac41754999d4d5324f6a12dfb9b6 (diff) | |
parent | af3b3680e090ad80ffeb6581f1f58f2dd4ae5195 (diff) | |
download | px4-firmware-ec740c5509632366e8e4a87ac9ae5731b6fb727c.tar.gz px4-firmware-ec740c5509632366e8e4a87ac9ae5731b6fb727c.tar.bz2 px4-firmware-ec740c5509632366e8e4a87ac9ae5731b6fb727c.zip |
Merge branch 'sf0x_paranoid'
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/sf0x/sf0x.cpp | 40 |
1 files changed, 19 insertions, 21 deletions
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 80ecab2ee..d382d08d0 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -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,15 +588,16 @@ 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') { - char buf[sizeof(_linebuf)]; - memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); - 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 */ - if (i > 0 && _linebuf[i] == '.') { + if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { valid = true; } } @@ -607,7 +605,7 @@ SF0X::collect() if (valid) { 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) { valid = true; } else { @@ -617,7 +615,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; @@ -709,12 +707,12 @@ SF0X::cycle() int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, - USEC2TICK(1100)); + USEC2TICK(1042 * 8)); return; } |