From 8d3f24424dc8fa1be3aba3159cfb4f5b6ada2ec3 Mon Sep 17 00:00:00 2001 From: Marcelo Aquino Date: Thu, 26 Feb 2015 21:19:47 -0300 Subject: [PATCH] fix mqtt gateway doesn't distribute node ids --- libraries/MySensors/MyMQTT.cpp | 226 ++++++++++-------- libraries/MySensors/MyMQTT.h | 2 +- .../examples/MQTTGateway/MQTTGateway.ino | 3 +- 3 files changed, 129 insertions(+), 102 deletions(-) diff --git a/libraries/MySensors/MyMQTT.cpp b/libraries/MySensors/MyMQTT.cpp index 610414e..fca2eea 100755 --- a/libraries/MySensors/MyMQTT.cpp +++ b/libraries/MySensors/MyMQTT.cpp @@ -13,7 +13,7 @@ version 2 as published by the Free Software Foundation. #include "utility/MsTimer2.h" char V_0[] PROGMEM = "TEMP"; //V_TEMP -char V_1[] PROGMEM = "HUM"; //V_HUM +char V_1[] PROGMEM = "HUM"; //V_HUM char V_2[] PROGMEM = "LIGHT"; //V_LIGHT char V_3[] PROGMEM = "DIMMER"; //V_DIMMER char V_4[] PROGMEM = "PRESSURE"; //V_PRESSURE @@ -23,7 +23,7 @@ char V_7[] PROGMEM = "RAINRATE"; //V_RAINRATE char V_8[] PROGMEM = "WIND"; //V_WIND char V_9[] PROGMEM = "GUST"; //V_GUST char V_10[] PROGMEM = "DIRECTON"; //V_DIRECTON -char V_11[] PROGMEM = "UV"; //V_UV +char V_11[] PROGMEM = "UV"; //V_UV char V_12[] PROGMEM = "WEIGHT"; //V_WEIGHT char V_13[] PROGMEM = "DISTANCE"; //V_DISTANCE char V_14[] PROGMEM = "IMPEDANCE"; //V_IMPEDANCE @@ -35,20 +35,20 @@ char V_19[] PROGMEM = "SCENE_ON"; //V_SCENE_ON char V_20[] PROGMEM = "SCENE_OFF"; //V_SCENE_OFF char V_21[] PROGMEM = "HEATER"; //V_HEATER char V_22[] PROGMEM = "HEATER_SW"; //V_HEATER_SW -char V_23[] PROGMEM = "LIGHT_LEVEL"; //V_LIGHT_LEVEL +char V_23[] PROGMEM = "LIGHT_LEVEL";//V_LIGHT_LEVEL char V_24[] PROGMEM = "VAR1"; //V_VAR1 char V_25[] PROGMEM = "VAR2"; //V_VAR2 char V_26[] PROGMEM = "VAR3"; //V_VAR3 char V_27[] PROGMEM = "VAR4"; //V_VAR4 char V_28[] PROGMEM = "VAR5"; //V_VAR5 -char V_29[] PROGMEM = "UP"; //V_UP +char V_29[] PROGMEM = "UP"; //V_UP char V_30[] PROGMEM = "DOWN"; //V_DOWN char V_31[] PROGMEM = "STOP"; //V_STOP char V_32[] PROGMEM = "IR_SEND"; //V_IR_SEND char V_33[] PROGMEM = "IR_RECEIVE"; //V_IR_RECEIVE char V_34[] PROGMEM = "FLOW"; //V_FLOW char V_35[] PROGMEM = "VOLUME"; //V_VOLUME -char V_36[] PROGMEM = "LOCK_STATUS"; //V_LOCK_STATUS +char V_36[] PROGMEM = "LOCK_STATUS";//V_LOCK_STATUS char V_37[] PROGMEM = "DUST_LEVEL"; //V_DUST_LEVEL char V_38[] PROGMEM = "VOLTAGE"; //V_VOLTAGE char V_39[] PROGMEM = "CURRENT"; //V_CURRENT @@ -72,10 +72,10 @@ char V_56[] PROGMEM = ""; // char V_57[] PROGMEM = ""; // char V_58[] PROGMEM = ""; // char V_59[] PROGMEM = ""; // -char V_60[] PROGMEM = "Started!\n"; //Custom for MQTTGateway +char V_60[] PROGMEM = "Started!\n"; //Custom for MQTTGateway char V_61[] PROGMEM = "SKETCH_NAME"; //Custom for MQTTGateway char V_62[] PROGMEM = "SKETCH_VERSION"; //Custom for MQTTGateway -char V_63[] PROGMEM = "UNKNOWN"; //Custom for MQTTGateway +char V_63[] PROGMEM = "UNKNOWN"; //Custom for MQTTGateway ////////////////////////////////////////////////////////////////// @@ -89,7 +89,6 @@ PROGMEM const char *vType[] = { V_61, V_62, V_63 }; - char broker[] PROGMEM = MQTT_BROKER_PREFIX; #define S_FIRSTCUSTOM 60 @@ -132,7 +131,7 @@ void MyMQTT::begin(rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_e dataR Serial.begin(BAUD_RATE); repeaterMode = true; isGateway = true; - MQTTClientConnected = false; + MQTTClients = 0; setupRepeaterMode(); dataCallback = inDataCallback; @@ -163,77 +162,120 @@ void MyMQTT::processRadioMessage() { if (process()) { // A new message was received from one of the sensors MyMessage message = getLastMessage(); - // Pass along the message from sensors to serial line rxBlink(1); - SendMQTT(message); - } + if (msg.isAck()) { + Serial.println("msg is ack!"); + if (msg.sender == 255 && mGetCommand(msg) == C_INTERNAL && msg.type == I_ID_REQUEST) { + // TODO: sending ACK request on id_response fucks node up. doesn't work. + // The idea was to confirm id and save to EEPROM_LATEST_NODE_ADDRESS. + } + } else { + // we have to check every message if its a newly assigned id or not. + // Ack on I_ID_RESPONSE does not work, and checking on C_PRESENTATION isn't reliable. + uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1; + if (newNodeID <= MQTT_FIRST_SENSORID) { + newNodeID = MQTT_FIRST_SENSORID; + } + if (msg.sender == newNodeID) { + saveState(EEPROM_LATEST_NODE_ADDRESS,newNodeID); + } + + if (mGetCommand(msg) == C_INTERNAL) { + if (msg.type == I_CONFIG) { + txBlink(1); + if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_CONFIG, 0).set("M"))) { + errBlink(1); + } + } else if (msg.type == I_ID_REQUEST && msg.sender == 255) { + uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1; + if (newNodeID <= MQTT_FIRST_SENSORID) { + newNodeID = MQTT_FIRST_SENSORID; + } + if (newNodeID >= MQTT_LAST_SENSORID) { + // Sorry no more id's left :( + newNodeID = AUTO; + } + txBlink(1); + if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_ID_RESPONSE, 0).set(newNodeID))) { + errBlink(1); + } + } + } else if (mGetCommand(msg)!= C_PRESENTATION) { + // Pass along the message from sensors to MQTT + SendMQTT(message); + } + } + } } void MyMQTT::processMQTTMessage(char *inputString, uint8_t inputPos) { - char *str, *p; + char *str, *p, *payload=NULL; uint8_t i = 0; buffer[0]= 0; buffsize = 0; + uint8_t mqttMsgType = (uint8_t)inputString[0] >> 4; - if ((uint8_t)inputString[0] >> 4 == MQTTCONNECT) { + if (mqttMsgType == MQTTCONNECT) { buffer[buffsize++] = MQTTCONNACK << 4; buffer[buffsize++] = 0x02; // Remaining length buffer[buffsize++] = 0x00; // Connection accepted buffer[buffsize++] = 0x00; // Reserved - MQTTClientConnected=true; // We have connection! - } - if ((uint8_t)inputString[0] >> 4 == MQTTPINGREQ) { + MQTTClients++; // We have a new client connected! + } else if (mqttMsgType == MQTTPINGREQ) { buffer[buffsize++] = MQTTPINGRESP << 4; buffer[buffsize++] = 0x00; - } - if ((uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE) { - buffer[buffsize++] = MQTTSUBACK << 4; // Just ack everything, we actually dont really care! - buffer[buffsize++] = 0x03; // Remaining length + } else if (mqttMsgType == MQTTSUBSCRIBE) { + buffer[buffsize++] = MQTTSUBACK << 4; // Just ack everything, we actually dont really care! + buffer[buffsize++] = 0x03; // Remaining length buffer[buffsize++] = (uint8_t)inputString[2]; // Message ID MSB buffer[buffsize++] = (uint8_t)inputString[3]; // Message ID LSB - buffer[buffsize++] = MQTTQOS0; // QOS level - } - if ((uint8_t)inputString[0] >> 4 == MQTTUNSUBSCRIBE) { + buffer[buffsize++] = MQTTQOS0; // QOS level + } else if (mqttMsgType == MQTTUNSUBSCRIBE) { buffer[buffsize++] = MQTTUNSUBACK << 4; - buffer[buffsize++] = 0x02; // Remaining length + buffer[buffsize++] = 0x02; // Remaining length buffer[buffsize++] = (uint8_t)inputString[2]; // Message ID MSB buffer[buffsize++] = (uint8_t)inputString[3]; // Message ID LSB + } else if (mqttMsgType == MQTTDISCONNECT) { + MQTTClients--; // Client disconnected! } - if ((uint8_t)inputString[0] >> 4 == MQTTDISCONNECT) { - MQTTClientConnected=false; // We lost connection! - } + if (buffsize > 0) { - dataCallback(buffer,&buffsize); + dataCallback(buffer, &buffsize); } // We publish everything we get, we dont care if its subscribed or not! - if ((uint8_t)inputString[0] >> 4 == MQTTPUBLISH || (MQTT_SEND_SUBSCRIPTION && (uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE)) { - buffer[0]= 0; + if (mqttMsgType == MQTTPUBLISH || (MQTT_SEND_SUBSCRIPTION && mqttMsgType == MQTTSUBSCRIBE)) { + buffer[0] = 0; buffsize = 0; // Cut out address and payload depending on message type. - if ((uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE) { - strncat(buffer,inputString+6,inputString[5]); + if (mqttMsgType == MQTTSUBSCRIBE) { + strncat(buffer, inputString+6, inputString[5]); } else { - strncat(buffer,inputString+4,inputString[3]); + strncat(buffer, inputString+4, inputString[3]); } // TODO: Check if we should send ack or not. - for (str = strtok_r(buffer,"/",&p) ; str && i<4 ; str = strtok_r(NULL,"/",&p)) { + for (str = strtok_r(buffer, "/", &p) ; str && i<4 ; str = strtok_r(NULL, "/", &p)) { if (i == 0) { - if (strcmp_P(str,broker)!=0) { //look for MQTT_BROKER_PREFIX - return; //Message not for us or malformatted! + //look for MQTT_BROKER_PREFIX + if (strcmp_P(str, broker) != 0) { + //Message not for us or malformatted! + return; } } else if (i==1) { - msg.destination = atoi(str); //NodeID + //NodeID + msg.destination = atoi(str); } else if (i==2) { - msg.sensor = atoi(str); //SensorID + //SensorID + msg.sensor = atoi(str); } else if (i==3) { - char match=0; //SensorType - //strcpy(str,(char*)&str[2]); //Strip V_ + //SensorType + char match=0; for (uint8_t j=0; strcpy_P(convBuf, (char*)pgm_read_word(&(vType[j]))) ; j++) { - if (strcmp((char*)&str[2],convBuf)==0) { //Strip V_ and compare + //Strip V_ and compare + if (strcmp((char*)&str[2], convBuf)==0) { match=j; break; } @@ -245,70 +287,58 @@ void MyMQTT::processMQTTMessage(char *inputString, uint8_t inputPos) { msg.type = match; } i++; - } //Check if packge has payload - if ((uint8_t)inputString[1] > (uint8_t)(inputString[3]+2) && !((uint8_t)inputString[0] >> 4 == MQTTSUBSCRIBE)) { - strcpy(convBuf,inputString+(inputString[3]+4)); - msg.set(convBuf); //Payload - } else { - msg.set(""); //No payload } + + // Check if package has payload + if (mqttMsgType == MQTTPUBLISH) { + uint8_t length = (uint8_t)inputString[1] - (uint8_t)(inputString[3]+2); + if (length && length < MAX_PAYLOAD*2) { + // Payload + memcpy(convBuf, inputString+(inputString[3]+4), length); + convBuf[length] = 0; + payload = convBuf; + } + } + msg.set(payload); + txBlink(1); if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.destination, msg.sensor, C_SET, msg.type, 0))) errBlink(1); - } } void MyMQTT::SendMQTT(MyMessage &msg) { buffsize = 0; - if (!MQTTClientConnected) return; //We have no connections - return - if (msg.isAck()) { - Serial.println("msg is ack!"); - if (msg.sender==255 && mGetCommand(msg)==C_INTERNAL && msg.type==I_ID_REQUEST) { - // TODO: sending ACK request on id_response fucks node up. doesn't work. - // The idea was to confirm id and save to EEPROM_LATEST_NODE_ADDRESS. - } - } else { - // we have to check every message if its a newly assigned id or not. - // Ack on I_ID_RESPONSE does not work, and checking on C_PRESENTATION isn't reliable. - uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1; - if (newNodeID <= MQTT_FIRST_SENSORID) newNodeID = MQTT_FIRST_SENSORID; - if (msg.sender==newNodeID) { - saveState(EEPROM_LATEST_NODE_ADDRESS,newNodeID); - } - if (mGetCommand(msg)==C_INTERNAL) { - if (msg.type==I_CONFIG) { - txBlink(1); - if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_CONFIG, 0).set("M"))) errBlink(1); - } else if (msg.type==I_ID_REQUEST && msg.sender==255) { - uint8_t newNodeID = loadState(EEPROM_LATEST_NODE_ADDRESS)+1; - if (newNodeID <= MQTT_FIRST_SENSORID) newNodeID = MQTT_FIRST_SENSORID; - if (newNodeID >= MQTT_LAST_SENSORID) return; // Sorry no more id's left :( - txBlink(1); - if (!sendRoute(build(msg, GATEWAY_ADDRESS, msg.sender, 255, C_INTERNAL, I_ID_RESPONSE, 0).set(newNodeID))) errBlink(1); - } - } else if (mGetCommand(msg)!=0) { - if (mGetCommand(msg)==3) msg.type=msg.type+(S_FIRSTCUSTOM-10); //Special message - - buffer[buffsize++] = MQTTPUBLISH << 4; // 0: - buffer[buffsize++] = 0x09; // 1: Remaining length with no payload, we'll set this later to correct value, buffsize -2 - buffer[buffsize++] = 0x00; // 2: Length MSB (Remaing length can never exceed ff,so MSB must be 0!) - buffer[buffsize++] = 0x08; // 3: Length LSB (ADDR), We'll set this later - if (msg.type > V_TOTAL) msg.type=V_TOTAL;// If type > defined types set to unknown. - strcpy_P(buffer+4, broker); - buffsize+=strlen_P(broker); - buffsize+=sprintf(&buffer[buffsize],"/%i/%i/V_%s",msg.sender,msg.sensor,getType(convBuf, &vType[msg.type])); - buffer[3]=buffsize-4; // Set correct address length on byte 4. -#ifdef DEBUG - Serial.println((char*)&buffer[4]); -#endif - msg.getString(convBuf); - for (uint8_t a=0; a V_TOTAL) { + // If type > defined types set to unknown. + msg.type=V_TOTAL; + } + strcpy_P(buffer+4, broker); + buffsize+=strlen_P(broker); + buffsize+=sprintf(&buffer[buffsize], "/%i/%i/V_%s", msg.sender, msg.sensor, getType(convBuf, &vType[msg.type])); + buffer[3]=buffsize-4; // Set correct address length on byte 4. +#ifdef DEBUG + Serial.println((char*)&buffer[4]); +#endif + msg.getString(convBuf); + for (uint8_t a=0; a