diff --git a/libraries/MySensors/MyGateway.cpp b/libraries/MySensors/MyGateway.cpp index 9040bbc..0a929be 100644 --- a/libraries/MySensors/MyGateway.cpp +++ b/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)); } diff --git a/libraries/MySensors/MyGateway.h b/libraries/MySensors/MyGateway.h index 185d933..e527bc3 100644 --- a/libraries/MySensors/MyGateway.h +++ b/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(); diff --git a/libraries/MySensors/MyMessage.cpp b/libraries/MySensors/MyMessage.cpp index 96ae96e..39c3cc4 100644 --- a/libraries/MySensors/MyMessage.cpp +++ b/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(); diff --git a/libraries/MySensors/MyMessage.h b/libraries/MySensors/MyMessage.h index a1999b6..d8df627 100644 --- a/libraries/MySensors/MyMessage.h +++ b/libraries/MySensors/MyMessage.h @@ -13,6 +13,8 @@ #define MyMessage_h #include +#include +#include #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;