Merge branch '#111-px4io-integrated-mixing' of github.com:PX4/Firmware into fixedwing_io_mixing

apps/px4io/mixer.cpp

@@ -235,11 +235,11 @@ mixer_handle_text(const void *buffer, size_t length)
 		if (resid != mixer_text_length) {
 			debug("used %u", mixer_text_length - resid);
 
-		// 	 copy any leftover text to the base of the buffer for re-use 
-		// 	if (resid > 0)
-		// 		memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
 
-		// 	mixer_text_length = resid;
+			mixer_text_length = resid;
 		}
 
 		break;

apps/systemlib/mixer/mixer_simple.cpp

@@ -72,17 +72,25 @@ SimpleMixer::~SimpleMixer()
 }
 
 static const char *
-skipspace(const char *p, unsigned &len)
+findtag(const char *buf, unsigned &buflen, char tag)
 {
-	while (isspace(*p)) {
-		if (len == 0)
-			return nullptr;
-
-		len--;
-		p++;
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
 	}
+	return nullptr;
+}
+
+static void
+skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
 
-	return p;
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
+		buflen -= (p - buf);
 }
 
 int
@@ -90,20 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 {
 	int ret;
 	int s[5];
-	int used;
-
-	buf = skipspace(buf, buflen);
-
-	if (buflen < 16)
+	
+	buf = findtag(buf, buflen, 'O');
+	if ((buf == nullptr) || (buflen < 12))
 		return -1;
 
-	if ((ret = sscanf(buf, "O: %d %d %d %d %d%n",
-			  &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) {
+	if ((ret = sscanf(buf, "O: %d %d %d %d %d",
+			  &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
 		debug("scaler parse failed on '%s' (got %d)", buf, ret);
 		return -1;
 	}
-
-	buflen -= used;
+	skipline(buf, buflen);
 
 	scaler.negative_scale	= s[0] / 10000.0f;
 	scaler.positive_scale	= s[1] / 10000.0f;
@@ -119,20 +124,17 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 {
 	unsigned u[2];
 	int s[5];
-	int used;
 
-	buf = skipspace(buf, buflen);
-
-	if (buflen < 16)
+	buf = findtag(buf, buflen, 'S');
+	if ((buf == nullptr) || (buflen < 16))
 		return -1;
 
-	if (sscanf(buf, "S: %u %u %d %d %d %d %d%n",
-		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4], &used) != 7) {
+	if (sscanf(buf, "S: %u %u %d %d %d %d %d",
+		   &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
 		debug("control parse failed on '%s'", buf);
 		return -1;
 	}
-
-	buflen -= used;
+	skipline(buf, buflen);
 
 	control_group		= u[0];
 	control_index		= u[1];