Binary message payload handling

libraries/MySensors/MyGateway.cpp

@@ -105,10 +105,22 @@ void MyGateway::checkInclusionFinished() {
 	 }
 }
 
+uint8_t MyGateway::h2i(char c) {
+	uint8_t i = 0;
+	if (c <= '9')
+		i += c - '0';
+	else if (c >= 'a')
+		i += c - 'a' + 10;
+	else
+		i += c - 'A' + 10;
+	return i;
+}
 
 void MyGateway::parseAndSend(char *commandBuffer) {
   boolean ok = false;
   char *str, *p, *value=NULL;
+  uint8_t bvalue[MAX_PAYLOAD];
+  uint8_t blen = 0;
   int i = 0;
   uint16_t destination = 0;
   uint8_t sensor = 0;
@@ -138,7 +150,18 @@ void MyGateway::parseAndSend(char *commandBuffer) {
 		type = atoi(str);
 		break;
 	  case 5: // Variable value
-		value = str;
+		if (command == C_STREAM) {
+			blen = 0;
+			uint8_t val;
+			while (*str) {
+				val = h2i(*str++) << 4;
+				val += h2i(*str++);
+				bvalue[blen] = val;
+				blen++;
+			}
+		} else {
+			value = str;
+		}
 		break;
 	  }
 	  i++;
@@ -161,7 +184,10 @@ void MyGateway::parseAndSend(char *commandBuffer) {
 	msg.type = type;
 	mSetCommand(msg,command);
 	mSetAck(msg,ack==0?false:true);
-	msg.set(value);
+	if (command == C_STREAM)
+		msg.set(bvalue, blen);
+	else
+		msg.set(value);
     ok = sendRoute(msg);
     if (!ok) {
       errBlink(1);
@@ -211,7 +237,7 @@ void MyGateway::serial(const char *fmt, ... ) {
 }
 
 void MyGateway::serial(MyMessage &msg) {
-  serial(PSTR("%d;%d;%d;%d;%s\n"),msg.sender, msg.sensor, mGetCommand(msg), msg.type, msg.getString(convBuf));
+  serial(PSTR("%d;%d;%d;%d;%s\n"),msg.sender, msg.sensor, mGetCommand(msg), msg.type, (mGetCommand(msg) == C_STREAM) ? msg.getStream(convBuf) : msg.getString(convBuf));
 }
 
 

libraries/MySensors/MyGateway.h

@@ -68,6 +68,8 @@ class MyGateway : public MySensor
 	    uint8_t pinTx;
 	    uint8_t pinEr;
 
+		uint8_t h2i(char c);
+
 	    void serial(const char *fmt, ... );
 	    void serial(MyMessage &msg);
 	    void interruptStartInclusion();

libraries/MySensors/MyMessage.cpp

@@ -28,6 +28,30 @@ const char* MyMessage::getString() const {
 	}
 }
 
+// handles single character hex (0 - 15)
+char MyMessage::i2h(uint8_t i) const {
+	uint8_t k = i & 0x0F;
+	if (k <= 9)
+		return '0' + k;
+	else
+		return 'A' + k - 10;
+}
+
+char* MyMessage::getStream(char *buffer) const {
+	uint8_t cmd = miGetCommand();
+	if ((cmd == C_STREAM) && (buffer != NULL)) {
+		for (uint8_t i = 0; i < miGetLength(); i++)
+		{
+			buffer[i * 2] = i2h(data[i] >> 4);
+			buffer[(i * 2) + 1] = i2h(data[i]);
+		}
+		buffer[miGetLength() * 2] = '\0';
+		return buffer;
+	} else {
+		return NULL;
+	}
+}
+
 char* MyMessage::getString(char *buffer) const {
 	uint8_t payloadType = miGetPayloadType();
 

libraries/MySensors/MyMessage.h

@@ -13,6 +13,8 @@
 #define MyMessage_h
 
 #include <SPI.h>
+#include <string.h>
+#include <stdint.h>
 
 #define PROTOCOL_VERSION 2
 #define MAX_MESSAGE_LENGTH 32
@@ -92,6 +94,7 @@ typedef enum {
 
 
 // internal access for special fields
+#define miGetCommand() BF_GET(command_ack_payload, 0, 3)
 #define miGetPayloadType() BF_GET(command_ack_payload, 4, 4)
 #define miGetLength() BF_GET(version_length, 3, 5)
 
@@ -107,11 +110,14 @@ public:
 
 	MyMessage(uint8_t sensor, uint8_t type);
 
+	char i2h(uint8_t i) const;
+
 	/**
 	 * If payload is something else than P_STRING you can have the payload value converted
 	 * into string representation by supplying a buffer with the minimum size of
 	 * 2*MAX_PAYLOAD+1. This is to be able to fit hex-conversion of a full binary payload.
 	 */
+	char* getStream(char *buffer) const;
 	char* getString(char *buffer) const;
 	const char* getString() const;
 	void* getCustom() const;