From 4183444de626698137ab3fbb413a96a806da3ade Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:50:00 +0200 Subject: Changed to proper parser for SF02/F laser rangefinder --- src/drivers/sf0x/module.mk | 3 +- src/drivers/sf0x/sf0x.cpp | 89 ++++++++-------------------------------- src/drivers/sf0x/sf0x_parser.cpp | 1 - 3 files changed, 18 insertions(+), 75 deletions(-) (limited to 'src/drivers/sf0x') diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk index dc2c66d56..26e77d1cc 100644 --- a/src/drivers/sf0x/module.mk +++ b/src/drivers/sf0x/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = sf0x -SRCS = sf0x.cpp +SRCS = sf0x.cpp \ + sf0x_parser.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d382d08d0..52f7413a9 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -72,6 +72,8 @@ #include +#include "sf0x_parser.h" + /* Configuration Constants */ /* oddly, ERROR is not defined for c++ */ @@ -120,6 +122,7 @@ private: int _fd; char _linebuf[10]; unsigned _linebuf_index; + enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; orb_advert_t _range_finder_topic; @@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) : _collect_phase(false), _fd(-1), _linebuf_index(0), + _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), _range_finder_topic(-1), _consecutive_fail_count(0), @@ -526,14 +530,14 @@ SF0X::collect() } /* the buffer for read chars is buflen minus null termination */ - unsigned readlen = sizeof(_linebuf) - 1; + char readbuf[20]; + unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ - ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); + ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { - _linebuf[sizeof(_linebuf) - 1] = '\0'; - debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); + debug("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -548,84 +552,23 @@ SF0X::collect() return -EAGAIN; } - /* let the write pointer point to the next free entry */ - _linebuf_index += ret; - _last_read = hrt_absolute_time(); - /* 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 - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - - if (_linebuf_index == readlen) { - /* we have a full buffer, but no line ending - abort */ - _linebuf_index = 0; - perf_count(_comms_errors); - return -ENOMEM; - } else { - /* incomplete read, reschedule ourselves */ - return -EAGAIN; - } - } - - char *end; float si_units; - bool valid; - - /* enforce line ending */ - _linebuf[_linebuf_index] = '\0'; - - if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { - si_units = -1.0f; - valid = false; - - } else { - - /* we need to find a dot in the string, as we're missing the meters part else */ - valid = false; - - /* wipe out partially read content from last cycle(s), check for dot */ - for (unsigned i = 0; i < (_linebuf_index - 2); i++) { - if (_linebuf[i] == '\n') { - /* 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 - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { - valid = true; - } - } - - if (valid) { - si_units = strtod(_linebuf, &end); - - /* we require at least four characters for a valid number */ - if (end > _linebuf + 3) { - valid = true; - } else { - si_units = -1.0f; - valid = false; - } + bool valid = false; + + for (unsigned i = 0; i < ret; i++) { + if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { + valid = true; } } - 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; - - /* if its invalid, there is no reason to forward the value */ if (!valid) { - perf_count(_comms_errors); - return -EINVAL; + return -EAGAIN; } + debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp index b9ceb52f5..cddc3fe99 100644 --- a/src/drivers/sf0x/sf0x_parser.cpp +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -63,7 +63,6 @@ int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PA int ret = -1; char *end; - unsigned len = strlen(parserbuf); switch (*state) { case SF0X_PARSE_STATE0_UNSYNC: if (c == '\n') { -- cgit v1.2.3