Sunspec heartbeat timer incrementation slowed down

PLC/POUs/Sunspec/FB_PowerSupplySunspec.TcPOU

@@ -97,6 +97,9 @@ VAR
 	// Timer for setting the inverter power
 	_tonSetPowerTimer : TON;
 	
+	// Timer for incrementing heartbeat signal
+	_tonHearbeatIncTimer : TON := (PT := T#500ms); 
+	
 	// Current DC values (DCA, DCA_SF, DCV, DCV_SF, DCW, DCW_SF) in word array for efficient modbus reading
 	_awCurrentDCValues : ARRAY[0..5] OF WORD;
 	
@@ -221,6 +224,13 @@ IF (rPower < -rMaxBattPower) THEN
 	rPower := -rMaxBattPower;
 END_IF
 
+// Increment heartbeat counter
+_tonHearbeatIncTimer(IN := TRUE);
+IF _tonHearbeatIncTimer.Q THEN
+	_tonHearbeatIncTimer(IN := FALSE);
+	_uiPLCToInverterCounter := _uiPLCToInverterCounter + 1;
+END_IF
+
 // State machine
 CASE _iState OF
 	
@@ -350,7 +360,6 @@ CASE _iState OF
 		
 		
 	5: // Send heartbeat signal
-		_uiPLCToInverterCounter := _uiPLCToInverterCounter + 1;
 		_fbWriteRegister(
 			sIPAddr:= sInverterIPAddr, 
 			nTCPPort:= 502,