Photon crashes with MQTT code

Here is my central heating system automation code, yesterday i tried to add MQTT support but when i send MQTT “ON” command particle crashes.
Maybe there’s some simple mistake that someone can spot?

when i send on/off/auto command from particle console: int MobileSwitch(String command)
then everything works (has been working for years).

SYSTEM_THREAD(ENABLED);

#include "mqtt.h"
#include "analogSmooth.h"
#include <LiquidCrystal_I2C_Spark.h>
#include <TimeAlarms.h>
#undef now();




float callback(char* topic, byte* payload, unsigned int length);

byte server[] = { 192,168,1,223 };
MQTT client(server, 1883, callback);

String writeAPIKey = "XXX";
String channelID = "XXX";
 
TCPClient Xclient;

int ledRed = A4; // A4  -- A0 if ON then indicates that there is 230V present and boiler pump is OFF
int ledGreen = A5; // A5 -- A1 is ON then boiler pump is ON
int radikatemp = A6; //A6 --A2

int toatemp = A0; //A0 -- A3
int pliiditemp = A1; //A1 -- A4
int pliidiveetemp = A2; //A2 -- A5
int xtemp = A3; //A3 --A6

int relayRadiator = D2; //relay to turn ON/OFF radiator pump
int relayBoiler = D3; // relay to turn ON/OFF boiler pump.



int c = 0;
int d = 0;
int aeg = 0;

int secs = 0;
int mins=0; 
int hours=0; 

LiquidCrystal_I2C *lcd;
//UDP UDPClient;

String timeStr;

AnalogSmooth as100 = AnalogSmooth(10);
AnalogSmooth as101 = AnalogSmooth(30);
AnalogSmooth as102 = AnalogSmooth(10);
AnalogSmooth as103 = AnalogSmooth(10);

float radikaC = 0.0;
float toaC = 0.0;
float pliidiC = 0.0;
float pliidiveeC = 0.0;
float xtempC = 0.0;

int toggleDelay = 5;
int toggleLCDState = 1;
int previousNow = 0;
int previousNow2 = 0;
int ForceState = 0;

float callback(char* topic, byte* payload, unsigned int length) {
    char p[length + 1];
    memcpy(p, payload, length);
    p[length] = NULL;
    Serial.println("callback...");
    
    if (!strcmp(p, "ON")){
        if(radikaC > 39){
            previousNow2 = Time.now();   
            ForceState = 1;
        }
    }        
    
            
    if (!strcmp(p, "OFF")){
        previousNow2 = Time.now();   
        ForceState = -1;
    }
    if (!strcmp(p, "AUTO")){
        ForceState = 0;
    }

}

void setup(){
    
  //  Particle.function("Radikapump",MobileSwitch);
    
    Time.zone(+2);
    Alarm.alarmRepeat(8,30,0, MorningAlarm);  // 8:30am every day
    Alarm.timerRepeat(400, Repeats2);// timer for every 400 seconds    
    Alarm.timerOnce(10, OnceOnly);             // called once after 10 seconds 
        
    Serial.begin(9600);

    lcd = new LiquidCrystal_I2C(0x27, 20, 4); // set the LCD address to 0x20 for a 16x2 //SparkCore bug, address is actually 27 but shifted (0x27*2)
    lcd->init(); // initialize the lcd
    lcd->backlight();
   // lcd->noBacklight();

    lcd->clear();
    
    pinMode(radikatemp, INPUT_PULLDOWN);
    pinMode(toatemp, INPUT_PULLDOWN);
    pinMode(pliiditemp, INPUT_PULLDOWN);
    pinMode(pliidiveetemp, INPUT_PULLDOWN);
    pinMode(xtemp, INPUT_PULLDOWN);
    
    pinMode(ledRed, OUTPUT);
    pinMode(ledGreen, OUTPUT);
    pinMode(relayRadiator, OUTPUT);
    pinMode(relayBoiler, OUTPUT);
    
 
    digitalWrite(relayRadiator, LOW);
    digitalWrite(relayBoiler, LOW);
    analogWrite(ledRed,150);
    
    }
    

void loop(){
    
    if(!client.isConnected()){
            client.connect("centralheating");
           // Serial.println("Connecting to MQTT...");
            client.subscribe("staatus");
        }
        
        if(client.isConnected()){
            client.loop();
           // Serial.println("Connected to MQTT.");
        }


    radikaC = (as100.analogReadSmooth(radikatemp) * 3.3) / 4095*100;  //getting the voltage reading from the temperature sensor
    toaC = (as101.analogReadSmooth(toatemp) * 3.3) / 4095*100 - 0.5;
    pliidiC = (as102.analogReadSmooth(pliiditemp) * 3.3) / 4095*200;
    pliidiveeC = (analogRead(pliidiveetemp) * 3.3) / 4095*100;
    xtempC = (analogRead(xtemp) * 3.3) / 4095*100;
    
    if(Time.now() - previousNow > toggleDelay) {

        previousNow = Time.now();   
        lcd->clear();

    
        if (toggleLCDState == 1){
            
            toggleLCDState = 0;
            toggleDelay = 3;
        }
          
        else{
        
            toggleLCDState = 1;
            toggleDelay = 10;
        }
    }
    
    if(Time.now() - previousNow2 > 7200) {

        previousNow2 = Time.now();   
        ForceState = 0;
        lcd->clear();
    }
    
        
    
    
    if(toggleLCDState == 1){
        lcd->setCursor(1,0);
        lcd->print("Korsten");
        lcd->setCursor(11,0);
        lcd->print(pliidiC, 1);
        lcd->print(" ");
        lcd->setCursor(1,1);
        lcd->print("Radikasse");
        lcd->setCursor(11,1);
        lcd->print(radikaC, 1);
        lcd->print(" ");
        lcd->setCursor(1,2);
        lcd->print("Toatemp");
        lcd->setCursor(11,2);
        lcd->print(toaC, 1);
        lcd->print(" ");
        lcd->setCursor(1,3);
        lcd->print("Pliita");
        lcd->setCursor(11,3);
        lcd->print(pliidiveeC, 1);
        lcd->print(" ");
    }
    
    else if(toggleLCDState == 0){
        
        digitalClockDisplay();
        
        if(ForceState != 0){
            secs = 14400 - (Time.now() - previousNow2); //
            mins=secs/60; //convert seconds to minutes
            hours=mins/60; //convert minutes to hours
        
            secs=secs-(mins*60); //subtract the coverted seconds to minutes in order to display 59 secs max 
            mins=mins-(hours*60); //subtract the coverted minutes to hours in order to display 59 minutes max
        
            lcd->setCursor(3,2);
            lcd->print("Forced state: ");
            lcd->setCursor(6,3);
            if(hours<10) lcd->print('0');
            lcd->print(hours);
            lcd->print(":");
            if(mins<10) lcd->print('0');
            lcd->print(mins);
            lcd->print(":");
            if(secs<10) lcd->print('0');
            lcd->print(secs); 
        }
    }
    
    if (toaC > 23.6){
        previousNow2 = Time.now();   
        ForceState = -1; 
    }
    
    c = tempkontroll(radikaC, toaC, c, aeg, pliidiveeC);
    d = tempkontroll2(pliidiC, pliidiveeC, d); 
    
    Taustavalgus();
    
    


    Alarm.delay(900); 
} 

void Taustavalgus(){
    
    if(Time.hour()<8 || Time.hour()>20){
        lcd->noBacklight();
    }
    else{
        lcd->backlight();
    }
}

void ThingSpeakUpdate(String tsData)
{
    Serial.println("Date string: " + tsData);
     
    Serial.println("...Connecting to Thingspeak");
     
    // Connecting and sending data to Thingspeak
    if(Xclient.connect("api.thingspeak.com", 80))
    {
        Serial.println("...Connection succesful, updating datastreams");
         
        Xclient.print("POST /update HTTP/1.1\n");
        Xclient.print("Host: api.thingspeak.com\n");
        Xclient.print("Connection: close\n");
        Xclient.print("X-THINGSPEAKAPIKEY: "+writeAPIKey+"\n");
        Xclient.print("Content-Type: application/x-www-form-urlencoded\n");
        Xclient.print("Content-Length: ");
        Xclient.print(tsData.length());
        Xclient.print("\n\n");
         
        Xclient.println(tsData); //the ""ln" is important here.
     
        // This delay is pivitol without it the TCP client will often close before the data is fully sent
        delay(250);
         
        Serial.println("Thingspeak update sent.");
    }
    else{
        Serial.println("Unable to connect to Thingspeak.");
    }
     
    if(!Xclient.connected()){
        Xclient.stop();
    }
    Xclient.flush();
    Xclient.stop();
}

int tempkontroll(float radikaC, float toaC, int c, int aeg, float pliidiveeC) {   
    
    aeg =Time.hour()*60+Time.minute(); 

    if((toaC < 20.7 && radikaC > 39 && c == 0  && aeg < 1290 && aeg > 360 && ForceState == 0) || (ForceState == 1)) {
            c = 1;
            digitalWrite(relayRadiator, HIGH); //paneb relee "HIGH" asendisse
        
    }
    /*
    
    if((radikaC > 84 || pliidiveeC > 84) && c == 0) {
            c = 1;
            digitalWrite(relayRadiator, HIGH); //paneb relee "HIGH" asendisse
            analogWrite(ledRed,150);
            Alarm.delay(100); 

    }
    
    */
    else if((((toaC > 21.4 || radikaC < 37) && c == 1 && radikaC < 88 && ForceState == 0) || (aeg > 1290  && radikaC < 88 && ForceState == 0)) || (ForceState == -1 && radikaC < 88) )  {
        c = 0;
        digitalWrite(relayRadiator, LOW); //paneb relee "LOW" asendisse
    } 
    return c;
    }

int tempkontroll2(float pliidiC, float pliidiveeC, int d) {   
    if((pliidiC > 90 || pliidiveeC > 80) && d == 0) {
        d = 1;
        digitalWrite(relayBoiler, HIGH); //paneb relee "HIGH" asendisse
        analogWrite(ledRed,0);
        analogWrite(ledGreen,150);
    }

    else if(pliidiC < 85 && pliidiveeC <79 && d == 1) {
        d = 0;
        digitalWrite(relayBoiler, LOW); //paneb relee "LOW" asendisse
        analogWrite(ledRed,150);
        analogWrite(ledGreen,0);
    } 
    return d;
}    

void MorningAlarm(){
    Spark.syncTime();
    if (isDST() == 1){
            
        Time.beginDST();
    }
    else {
            
        Time.endDST();
    }
}


void Repeats2(){
    if(client.isConnected()){
       // client.publish("centralheating/",String(celsius, 1));
    }
    
    if(Spark.connected())
    {
        ThingSpeakUpdate("field1="+String(toaC)+"&field2="+String(radikaC)+"&field3="+String(pliidiC)+"&field4="+String(pliidiveeC)+"&field6="+String(c)+"&field7="+String(d));
    }
}

void OnceOnly(){
    Spark.syncTime();
    if (isDST() == 1){
            
        Time.beginDST();
    }
    else {
            
        Time.endDST();
    }
}
    
bool isDST()
{ // (Central) European Summer Timer calculation (last Sunday in March/October)
    int dayOfMonth = Time.day();
    int month = Time.month();
    int dayOfWeek = Time.weekday() - 1; // make Sunday 0 .. Saturday 6
    
    if (month >= 4 && month <= 9){ // April to September definetly DST
        return true;
    }
    
    else if (month < 3 || month > 10){ // before March or after October is definetly standard time
        return false;
    }
    
    // March and October need deeper examination
    boolean lastSundayOrAfter = (dayOfMonth - dayOfWeek > 24);
    if (!lastSundayOrAfter){ // before switching Sunday
        return (month == 10); // October DST will be true, March not
    }
    
    if (dayOfWeek){ // AFTER the switching Sunday
        return (month == 3); // for March DST is true, for October not
    }
    
    int secSinceMidnightUTC = Time.now() % 86400;
    boolean dayStartedAs = (month == 10); // DST in October, in March not
    // on switching Sunday we need to consider the time
    if (secSinceMidnightUTC >= 1*3600){ // 1:00 UTC (=1:00 GMT/2:00 BST or 2:00 CET/3:00 CEST)
        return !dayStartedAs;
    }
    
    return dayStartedAs;
}    

void digitalClockDisplay(){

    lcd->setCursor(1,1);
    lcd->print(Time.year(), DEC);
    lcd->print('/');
    if(Time.month()<10) lcd->print('0');
    lcd->print(Time.month(), DEC);
    lcd->print('/');
    lcd->print(Time.day(), DEC);
    lcd->print(' ');
    if(Time.hour()<10) lcd->print('0');
    lcd->print(Time.hour(), DEC);
    lcd->print(':');
    if(Time.minute()<10) lcd->print('0');
    lcd->print(Time.minute(), DEC);
    lcd->print(':');
    if(Time.second()<10) lcd->print('0');
    lcd->print(Time.second(), DEC);
    	
}
    
int MobileSwitch(String command) {


    if (command=="off") {
        
        previousNow2 = Time.now();   
        ForceState = -1;
        return 0;
    }
    else if (command=="on") {
        
        if(radikaC > 39){
            
            previousNow2 = Time.now();   
            ForceState = 1;
            //digitalWrite(relayRadiator, HIGH);
            return 1;
        }
        else{
            return -1;
        }
        
    }
    else if (command=="auto") {
        
        ForceState = 0;
        return 1;
    }    
    else {
        return -1;
    }
}

MQTT callback function (i changed library void callback to float callback so i could retrieve values from it, it works on my other project):

float callback(char* topic, byte* payload, unsigned int length) {
    char p[length + 1];
    memcpy(p, payload, length);
    p[length] = NULL;
    Serial.println("callback...");
    
    if (!strcmp(p, "ON")){
        if(radikaC > 39){
            previousNow2 = Time.now();   
            ForceState = 1;
        }
    }        
    
            
    if (!strcmp(p, "OFF")){
        previousNow2 = Time.now();   
        ForceState = -1;
    }
    if (!strcmp(p, "AUTO")){
        ForceState = 0;
    }

}

Part of code that starts/stops pump:

int tempkontroll(float radikaC, float toaC, int c, int aeg, float pliidiveeC) {   
    
    aeg =Time.hour()*60+Time.minute(); 

    if((toaC < 20.7 && radikaC > 39 && c == 0  && aeg < 1290 && aeg > 360 && ForceState == 0) || (ForceState == 1)) {
            c = 1;
            digitalWrite(relayRadiator, HIGH); //paneb relee "HIGH" asendisse
        
    }
    
    else if((((toaC > 21.4 || radikaC < 37) && c == 1 && radikaC < 88 && ForceState == 0) || (aeg > 1290  && radikaC < 88 && ForceState == 0)) || (ForceState == -1 && radikaC < 88) )  {
        c = 0;
        digitalWrite(relayRadiator, LOW); //paneb relee "LOW" asendisse
    } 
    return c;
    }