void MSP_parser(byte* buffer, uint8_t size, uint8_t cmd, uint16_t listener_id) { uint8_t msg[size+2]; uint16_t msgId; uint8_t i = 2; switch (cmd) { /*PARSER_IMPLEMENTATION*/ } msg[0] = msgId >> 8; msg[1] = msgId & 0xFF; externalMessageEnqueue(msg, size+2, listener_id); }
void MQTT_message_callback(void *obj, const struct mosquitto_message *message) { printf("[MQTT] msg callback\n"); printf("%s %s\n", message->topic, message->payload); int len = strlen(message->payload); printf("[MQTT] receveid l:%i\n", len); if (((len % 3) == 0) && (len != 0)) { unsigned char msg[len % 3]; unsigned char * p = message->payload; int buf = 0; int index = 0; bool everythingisfine = true; while ((index < len) && everythingisfine) { if((*p - 48) < 10) { buf = (*p - 48) + 10 * buf; } else { everythingisfine = false; } if ((index % 3) == 2) { if(buf < 256) { msg[(index-2) / 3] = (uint8_t) buf; } else { everythingisfine = false; } buf = 0; } index++; p++; } if(everythingisfine) { printf("[MQTT] Message enqueue?\n"); externalMessageEnqueue(msg, (len / 3), MQTT_instance.listener_id); printf("[MQTT] Message received\n"); } else { printf("[MQTT] incorrect message '%s'\n", message->payload); } } else { printf("[MQTT] incorrect message '%s'\n", message->payload); } }
void Serial1_start_receiver_process() { printf("[Serial1] Starting receiver process\n"); int device = Serial1_device_id; int serialListenerState = Serial1_LISTENER_STATE_IDLE; char serialBuffer[Serial1_MAX_MSG_LENGTH]; int serialMsgSize = 0; char buffer[Serial1_INPUT_BUFFER_SIZE]; // Data read from the ESUSMS device int n; // used to store the results of select and read int i; // loop index while (1) { fd_set rdfs; // The file descriptor to wait on FD_ZERO( &rdfs ); FD_SET(device, &rdfs ); // set to the esusms fd n = select(device + 1, &rdfs, NULL, NULL, NULL); // NO Timeout here (last parameter) if (n < 0) { printf("[Serial1] Error waiting for incoming data from Serial device\n"); perror("Error waiting for incoming data from Serial device"); break; } else if (n == 0) { // timeout printf("[Serial1] Timeout waiting for incoming data from Serial device\n"); break; } else { // there is something to read n = read(device, &buffer, Serial1_INPUT_BUFFER_SIZE * sizeof(char)); //printf(" n=<%i>\n", n); if (n<0) { printf("[Serial1] Error reading from Serial device\n"); perror("Error reading from Serial device"); break; } else if (n==0) { printf("[Serial1] Nothing to read from Serial device\n"); break; } else { // There are n incoming bytes in buffer for (i = 0; i<n; i++) { //printf("[Serial1] byte received:%i\n", buffer[i]); switch(serialListenerState) { case Serial1_LISTENER_STATE_IDLE: //printf("[Serial1] State:IDLE\n"); if(buffer[i] == Serial1_START_BYTE) { serialListenerState = Serial1_LISTENER_STATE_READING; serialMsgSize = 0; } break; case Serial1_LISTENER_STATE_READING: //printf("[Serial1] State:READING\n"); if (serialMsgSize > Serial1_MAX_MSG_LENGTH) { serialListenerState = Serial1_LISTENER_STATE_ERROR; } else { if(buffer[i] == Serial1_STOP_BYTE) { serialListenerState = Serial1_LISTENER_STATE_IDLE; //printf("[Serial1] Message received\n"); externalMessageEnqueue(serialBuffer, serialMsgSize, Serial1_instance.listener_id); } else if (buffer[i] == Serial1_ESCAPE_BYTE) { serialListenerState = Serial1_LISTENER_STATE_ESCAPE; } else { serialBuffer[serialMsgSize] = buffer[i]; serialMsgSize++; } } break; case Serial1_LISTENER_STATE_ESCAPE: //printf("[Serial1] State:ESCAPE\n"); if (serialMsgSize > Serial1_MAX_MSG_LENGTH) { serialListenerState = Serial1_LISTENER_STATE_ERROR; } else { serialBuffer[serialMsgSize] = buffer[i]; serialMsgSize++; serialListenerState = Serial1_LISTENER_STATE_READING; } break; case Serial1_LISTENER_STATE_ERROR: printf("[Serial1] Message error: Too long\n"); serialListenerState = Serial1_LISTENER_STATE_IDLE; serialMsgSize = 0; break; } } } } } }