TOF Water tank sensor on KNX

#include <Arduino.h>
#include <KnxTpUart.h>
#include <Wire.h>
#include <VL53L0X.h>
#include <Smoothed.h>

#define DEBUG_Serial

#define LEDPIN 8
#define GPIO_AS_GND 4
#define SDA_PIN 6
#define SCL_PIN 7

#define HOME_ADRESS "7.1.1"
#define VALUE_Adress "6/4/8"
#define ERROR_Adress "6/4/9"

#define BASE_Interval 1000  //1sec
#define VALUES_Smoothed 10
#define SEC_PER_TX 30 // multiply base intervall
#define MAX_DISTANCE_VALUE 3000 // Max Distance is < 3000mm

KnxTpUart knx(&Serial0, HOME_ADRESS);
unsigned long previousMillis = 0;
VL53L0X sensor;
uint16_t measurementValue = 0;
Smoothed <int> sValue;
uint8_t count = 0;
bool initBool = true;
bool success = false;
uint8_t sensorTimeout = 0;
uint8_t sensorOutOfRange = 0;


void maintainKnxSerial() 
{
  if (Serial0.available() > 0) {
    KnxTpUartSerialEventType eType = knx.serialEvent();
    if (eType == KNX_TELEGRAM) {
      KnxTelegram* telegram = knx.getReceivedTelegram();

      String target =
        String(0 + telegram->getTargetMainGroup()) + "/" +
        String(0 + telegram->getTargetMiddleGroup()) + "/" +
        String(0 + telegram->getTargetSubGroup());
      
      Serial.println(target);

      if (telegram->getCommand() == KNX_COMMAND_READ) {
        // Answer to Read
        if (target == ERROR_Adress) {
          knx.groupAnswer14ByteText(ERROR_Adress, "Read Answer");
          digitalWrite(LEDPIN,!digitalRead(LEDPIN));
          #ifdef DEBUG_Serial
            Serial.println("KNX Read Command");
          #endif
        }
      } else if (telegram->getCommand() == KNX_COMMAND_WRITE) {
        // Write commands
        if (target == ERROR_Adress) {
          digitalWrite(LEDPIN,!digitalRead(LEDPIN));
          
          #ifdef DEBUG_Serial
            Serial.println("KNX Write Command");
          #endif
        } 
      }
    }
  }
}

void setup()
{
  pinMode(LEDPIN, OUTPUT);
  digitalWrite(LEDPIN, LOW);// ON at start --> LED Low active
  pinMode(GPIO_AS_GND,OUTPUT);
  digitalWrite(GPIO_AS_GND,LOW);

  #ifdef DEBUG_Serial
    Serial.begin(460800);
    Serial.println("Booting...");
  #endif

  Serial0.begin(19200,SERIAL_8E1,-1,-1);

  knx.uartReset();
  knx.addListenGroupAddress(ERROR_Adress);
  delay(300);
  knx.groupWrite14ByteText(ERROR_Adress, "KNX Start");
  #ifdef DEBUG_Serial
    Serial.println("KNX Start");
  #endif  

  Wire.setPins(SDA_PIN, SCL_PIN);
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    uint8_t counter = 0;
    while (initBool){
      
      if (sensor.init())
      {
        initBool = false;
      }else
      { 
        #ifdef DEBUG_Serial
          Serial.println("Init Error");
        #endif
        delay(200);
        knx.groupWrite14ByteText(ERROR_Adress, "Init Err");
        
        counter++;
        if (counter > 10)
        {
          digitalWrite(LEDPIN, true);
          delay(500);
          digitalWrite(LEDPIN, false);
          delay(500);
          digitalWrite(LEDPIN, true);
          delay(500);
          digitalWrite(LEDPIN, false);
          delay(500);
          digitalWrite(LEDPIN, true);
          delay(500);
          digitalWrite(LEDPIN, false);
          delay(500);
          digitalWrite(LEDPIN, true);
          delay(500);
          digitalWrite(LEDPIN, false);
          delay(500);
          digitalWrite(LEDPIN, true);
          delay(500);
          digitalWrite(LEDPIN, false);
          ESP.restart();
        }

        delay(BASE_Interval * SEC_PER_TX);
      }
    }
  }

  sensor.setSignalRateLimit(0.1);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);

  sValue.begin(SMOOTHED_EXPONENTIAL, VALUES_Smoothed);

  digitalWrite(LEDPIN, true);
  knx.groupWrite14ByteText(ERROR_Adress, "Init Done"); 

}

void loop()
{
  if(millis() - previousMillis > BASE_Interval)
  {
    previousMillis = millis();
    count++;
    digitalWrite(LEDPIN, !digitalRead(LEDPIN));
    measurementValue = sensor.readRangeSingleMillimeters();

    if (sensor.timeoutOccurred())
    {
      sensorTimeout++;
      digitalWrite(LEDPIN, false);
      #ifdef DEBUG_Serial
          Serial.println("Timeout Sensor");
      #endif   
    }else
    {
      if (measurementValue > MAX_DISTANCE_VALUE)
      {
        sensorOutOfRange++;
        digitalWrite(LEDPIN, false);
        #ifdef DEBUG_Serial
            Serial.printf("Sensor Out of Range, Value: %d \n", measurementValue);
        #endif

      }else
      {
        sValue.add((int)measurementValue);
        #ifdef DEBUG_Serial
            Serial.printf("Value: %d , Filtered: %d \n", (int)measurementValue, sValue.get());
        #endif
      }    
    }

    if (count >= SEC_PER_TX)
    {
      if ((sensorTimeout != 0)||(sensorOutOfRange != 0))
      {       
        String s = "L: " + String(sensorTimeout) + "; O: " + String(sensorOutOfRange);
        success = knx.groupWrite14ByteText(ERROR_Adress, s);
        sensorTimeout = 0;
        sensorOutOfRange = 0;
      }
      if(count > (sensorOutOfRange + sensorTimeout))
      {
        success = knx.groupWrite2ByteInt(VALUE_Adress, sValue.get());
      }
      count = 0;
    }
  }
  maintainKnxSerial();
  
}