GSM MQTT+CMT(SMS Reading) - c++

Question

I'm working a code to pubsub over GSM SIM800L and ARDUINO MEGA.
I'm able to workout MQTT fine, using ElementzOnline / SIM800_MQTT code.
I'm now having trouble to use MQTT alongside SMSReading (+CMT) and Call Receiving. I've made a RecSMS() to receive incoming messages, and it works fine as long as TCP Connection is not true, The problem is: Once TCP/MQTT is connected, my RecSMS Function displays no more sms to Serial and calls can not be received
This is Call Receiving, which is handled via RingPing....
void GSM_MQTT::receivecall(){
HangUp=1;
if(MQTT.TCP_Flag==true){disconnect();}
delay(1000);
SIMSerial.write("AT+CGATT=0\n");
delay(1000);
//SIMSerial.write("AT+CIPSHUT\n");
modemStatus=0;
SIMSerial.write("ATA\n");SIMSerial.write(26);Monitor("Incomig Call");
}
void GSM_MQTT::endcall(){if(HangUp==1){HangUp=0;SIMSerial.write("ATH\n");SIMSerial.write(26);Monitor("Call Ended");}}
This is RecSMS():
char inchar4;char inchar3;char inchar2;char inchar1;int times=0;
void RecSMS(char data){
if(times==3){inchar4=data;times++;}
if(times==2){inchar3=data;times++;}
if(times==1){inchar2=data;times++;}
if(times==0){inchar1=data;times++;}
if(times>=4){
inchar1=inchar2;inchar2=inchar3;inchar3=inchar4;inchar4=data;
if((inchar1== '+') && (inchar2== 'C')&&(inchar3== 'M')&&(inchar4== 'T')){RcvdConf = 1;}
if(RcvdConf == 1){
if(data == '\n'){RcvdEnd++;}
if(RcvdEnd == 3){RcvdEnd = 0;}
RcvdMsg[count] = data;
count++;
if(RcvdEnd == 2){RcvdConf = 0;MsgLength = count-2;count= 0;}
if(RcvdConf == 0){
Serial.print("Mobile Number is: ");
for(int x = 4;x < 18;x++){MsgMob[x-4] = RcvdMsg[x];Serial.print(MsgMob[x-4]);}
Serial.println();
Serial.print("Message Text: ");
for(int x = 47; x < MsgLength; x++){MsgTxt[x-47] = RcvdMsg[x];Serial.print(MsgTxt[x-47]);}
Serial.println();
char RcvdMsg[200] = "";int RcvdConf = 0;int count = 0;int RcvdEnd = 0;char MsgMob[15];char MsgTxt[50];int MsgLength = 0;
}
}
}
}
SerialEvent() is called every second...
void serialEvent(){
while(Serial3.available()){
char inChar=(char)Serial3.read();
RecSMS(inChar);//<////////////////////////sms reading
if(MQTT.TCP_Flag==false){
if(MQTT.index<200){MQTT.inputString[MQTT.index++]=inChar;}
if(inChar=='\n'){
MQTT.inputString[MQTT.index]=0;stringComplete=true;Serial.print(MQTT.inputString);
if(strstr(MQTT.inputString,MQTT.reply)!=NULL){
MQTT.GSM_ReplyFlag=1;
if(strstr(MQTT.inputString," INITIAL")!=0){MQTT.GSM_ReplyFlag=2;}
else if(strstr(MQTT.inputString," START")!=0){MQTT.GSM_ReplyFlag=3;}
else if(strstr(MQTT.inputString," IP CONFIG")!=0){_delay_us(10);MQTT.GSM_ReplyFlag=4;}
else if(strstr(MQTT.inputString," GPRSACT")!=0){MQTT.GSM_ReplyFlag=4;}
else if((strstr(MQTT.inputString," STATUS")!=0)||(strstr(MQTT.inputString,"TCP CLOSED")!=0)){MQTT.GSM_ReplyFlag=5;}
else if(strstr(MQTT.inputString," TCP CONNECTING")!=0){MQTT.GSM_ReplyFlag=6;}
else if((strstr(MQTT.inputString," CONNECT OK")!=0)||(strstr(MQTT.inputString,"CONNECT FAIL")!=NULL)||(strstr(MQTT.inputString,"PDP DEACT")!=0)){MQTT.GSM_ReplyFlag=7;}}
else if(strstr(MQTT.inputString,"OK")!=NULL){GSM_Response=1;}
else if(strstr(MQTT.inputString,"ERROR")!=NULL){GSM_Response=2;}
else if(strstr(MQTT.inputString,".")!= NULL){GSM_Response=3;}
else if(strstr(MQTT.inputString,"CONNECT FAIL")!=NULL){GSM_Response=5;}
else if(strstr(MQTT.inputString,"CONNECT")!=NULL){GSM_Response=4;MQTT.TCP_Flag=true;MQTT.AutoConnect();MQTT.pingFlag=true;MQTT.tcpATerrorcount=0;Serial.println("MQTT.TCP_Flag=True");}
else if(strstr(MQTT.inputString,"CLOSED")!=NULL){GSM_Response=4;MQTT.TCP_Flag=false;MQTT.MQTT_Flag=false;Serial.println("TCP_Flag=False");}
MQTT.index=0;MQTT.inputString[0]=0;
}
}else{
uint8_t ReceivedMessageType=(inChar/16) & 0x0F;uint8_t DUP=(inChar & DUP_Mask)/DUP_Mask;
uint8_t QoS=(inChar & QoS_Mask)/QoS_Scale;uint8_t RETAIN=(inChar & RETAIN_Mask);
if((ReceivedMessageType>=CONNECT)&&(ReceivedMessageType<=DISCONNECT)){
bool NextLengthByte=true;MQTT.length=0;MQTT.lengthLocal=0;uint32_t multiplier=1;delay(2);char Cchar=inChar;
while((NextLengthByte==true)&&(MQTT.TCP_Flag==true)){
if(Serial3.available()){
inChar=(char)Serial3.read();/*Serial.println(inChar, DEC);*/
if(((((Cchar & 0xFF)=='C')&&((inChar & 0xFF)=='L'))||(((Cchar & 0xFF)=='+')&&((inChar & 0xFF)=='P')))&&(MQTT.length==0)){
MQTT.index=0;MQTT.inputString[MQTT.index++]=Cchar;MQTT.inputString[MQTT.index++]=inChar;
MQTT.TCP_Flag=false;MQTT.MQTT_Flag=false;MQTT.pingFlag=false;Serial.println("Disconnecting");
Serial.println("MQTT.TCP_Flag=False (ln223)");
}else{
if((inChar&128)==128){MQTT.length+=(inChar&127)*multiplier;multiplier*=128;Serial.println("More");}
else{NextLengthByte=false;MQTT.length+=(inChar&127)*multiplier;multiplier*=128;}
}
}
}
MQTT.lengthLocal=MQTT.length;/*Serial.println(MQTT.length);*/
if(MQTT.TCP_Flag==true){
MQTT.printMessageType(ReceivedMessageType);MQTT.index=0L;uint32_t a=0;
while((MQTT.length-- > 0)&&(Serial3.available())){MQTT.inputString[uint32_t(MQTT.index++)]=(char)Serial3.read();delay(1);}
/*Serial.println(" ");*/
if (ReceivedMessageType==CONNACK){
MQTT.ConnectionAcknowledgement=MQTT.inputString[0]*256+MQTT.inputString[1];
if(MQTT.ConnectionAcknowledgement==0){MQTT.MQTT_Flag=true;MQTT.OnConnect();}
MQTT.printConnectAck(MQTT.ConnectionAcknowledgement); /*MQTT.OnConnect();*/
}else if(ReceivedMessageType==PUBLISH){
uint32_t TopicLength=(MQTT.inputString[0])*256+(MQTT.inputString[1]);Serial.print("Topic: '");MQTT.PublishIndex = 0;
for(uint32_t iter=2;iter<TopicLength+2;iter++){Serial.print(MQTT.inputString[iter]);MQTT.Topic[MQTT.PublishIndex++]=MQTT.inputString[iter];}
MQTT.Topic[MQTT.PublishIndex]=0;Serial.print("' Message: '");
MQTT.TopicLength=MQTT.PublishIndex;MQTT.PublishIndex=0;uint32_t MessageSTART=TopicLength+2UL;int MessageID=0;
if(QoS!=0){MessageSTART+=2;MessageID=MQTT.inputString[TopicLength+2UL]*256+MQTT.inputString[TopicLength+3UL];}
for(uint32_t iter=(MessageSTART);iter<(MQTT.lengthLocal);iter++){Serial.print(MQTT.inputString[iter]);MQTT.Message[MQTT.PublishIndex++]=MQTT.inputString[iter];}
MQTT.Message[MQTT.PublishIndex]=0;Serial.println("'");MQTT.MessageLength=MQTT.PublishIndex;
if(QoS==1){MQTT.publishACK(MessageID);}else if(QoS==2){MQTT.publishREC(MessageID);}
MQTT.OnMessage(MQTT.Topic,MQTT.TopicLength,MQTT.Message,MQTT.MessageLength);MQTT.MessageFlag=true;
}else if(ReceivedMessageType==PUBREC){MQTT.publishREL(0,MQTT.inputString[0]*256+MQTT.inputString[1]);
int MessageID=MQTT.inputString[0]*256+MQTT.inputString[1];
}else if(ReceivedMessageType==PUBREL){MQTT.publishCOMP(MQTT.inputString[0]*256+MQTT.inputString[1]);
int MessageID=MQTT.inputString[0]*256+MQTT.inputString[1];
}else if((ReceivedMessageType==PUBACK)||(ReceivedMessageType==PUBCOMP)||(ReceivedMessageType==SUBACK)||(ReceivedMessageType==UNSUBACK)){
int MessageID=MQTT.inputString[0]*256+MQTT.inputString[1];
}else if(ReceivedMessageType==PINGREQ){
MQTT.TCP_Flag=false;MQTT.pingFlag=false;MQTT.sendATreply("AT+CIPSHUT\r\n",".",4000);MQTT.modemStatus=0;
Serial.println("Disconnecting");Serial.println("MQTT.TCP_Flag=False (ln261)");
}
}
}else if((inChar==13)||(inChar==10)){Serial.print("inChar=13||10");}else{Serial.print("Received: Unknown Message Type: ");Serial.println(inChar);}
}
}
}
My Code is published at pedromancuso/GSM_MQTT.
Suggestions?