Serial.print() have to be: comms->print()

CmdMessenger.cpp

@@ -65,6 +65,7 @@ CmdMessenger::CmdMessenger(Stream &ccomms, const char fld_separator, const char
  */
 void CmdMessenger::init(Stream &ccomms, const char fld_separator, const char cmd_separator, const char esc_character)
 {
+	startCommand = false;
 	default_callback = NULL;
 	comms = &ccomms;
 	print_newlines = false;
@@ -481,15 +482,16 @@ double CmdMessenger::readDoubleArg()
  * Read next argument as string.
  * Note that the String is valid until the current command is replaced
  */
-char* CmdMessenger::readStringArg()
+const char* CmdMessenger::readStringArg()
 {
 	if (next()) {
 		dumped = true;
 		ArgOk = true;
+		unescape(current);
 		return current;
 	}
 	ArgOk = false;
-	return '\0';
+	return "\0";
 }
 
 /**
@@ -501,6 +503,7 @@ void CmdMessenger::copyStringArg(char *string, uint8_t size)
 	if (next()) {
 		dumped = true;
 		ArgOk = true;
+		unescape(current);
 		strlcpy(string, current, size);
 	}
 	else {
@@ -515,6 +518,7 @@ void CmdMessenger::copyStringArg(char *string, uint8_t size)
 uint8_t CmdMessenger::compareStringArg(char *string)
 {
 	if (next()) {
+		unescape(current);
 		if (strcmp(string, current) == 0) {
 			dumped = true;
 			ArgOk = true;
@@ -572,7 +576,8 @@ char* CmdMessenger::split_r(char *str, const char delim, char **nextp)
 	// Set start of return pointer to this position
 	ret = str;
 	// Find next delimiter
-	str += findNext(str, delim);
+	LastArgLength = findNext(str, delim);
+	str += LastArgLength;
 	// and exchange this for a a \0 char. This will terminate the char
 	if (*str) {
 		*str++ = '\0';
@@ -628,20 +633,20 @@ void CmdMessenger::printSci(double f, unsigned int digits)
 	// handle sign
 	if (f < 0.0)
 	{
-		Serial.print('-');
+		comms->print('-');
 		f = -f;
 	}
 
 	// handle infinite values
 	if (isinf(f))
 	{
-		Serial.print("INF");
+		comms->print("INF");
 		return;
 	}
 	// handle Not a Number
 	if (isnan(f))
 	{
-		Serial.print("NaN");
+		comms->print("NaN");
 		return;
 	}
 
@@ -675,4 +680,4 @@ void CmdMessenger::printSci(double f, unsigned int digits)
 	char output[16];
 	sprintf(output, format, whole, part, exponent);
 	comms->print(output);
-}
+}

CmdMessenger.h

@@ -66,6 +66,7 @@ class CmdMessenger
 	uint8_t bufferIndex;              // Index where to write data in buffer
 	uint8_t bufferLength;             // Is set to MESSENGERBUFFERSIZE
 	uint8_t bufferLastIndex;          // The last index of the buffer
+	uint8_t LastArgLength;             //The length if the last received argument
 	char ArglastChar;                 // Bookkeeping of argument escape char 
 	char CmdlastChar;                 // Bookkeeping of command escape char 
 	bool pauseProcessing;             // pauses processing of new commands, during sending
@@ -131,7 +132,9 @@ class CmdMessenger
 		byte *bytePointer = (byte *)(const void *)&value;
 		for (unsigned int i = 0; i < sizeof(value); i++)
 		{
-			*bytePointer = str[i];
+			*bytePointer = 0;
+			if( i < LastArgLength )
+				*bytePointer = str[i];
 			bytePointer++;
 		}
 		return value;
@@ -273,7 +276,7 @@ class CmdMessenger
 	char readCharArg();
 	float readFloatArg();
 	double readDoubleArg();
-	char *readStringArg();
+	const char *readStringArg();
 	void copyStringArg(char *string, uint8_t size);
 	uint8_t compareStringArg(char *string);
 
@@ -284,9 +287,11 @@ class CmdMessenger
 	{
 		if (next()) {
 			dumped = true;
+			ArgOk = true;
 			return readBin < T >(current);
 		}
 		else {
+			ArgOk = false;
 			return empty < T >();
 		}
 	}