SF02/F driver: Improve parsing

1 files changed, 16 insertions, 24 deletions

diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index d7780e9e8..cf0419075 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,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;