 #ifdef DEBUG
 unsigned long time;
 #endif
+
+// Initialize BMP280 sensor
+void initBMP() {
+  if (bmp.begin(BMP_ADDR)) {
+    DEBUG_PRINTLN("BMP280 sensor initialized");
+    bmpFound = true;
+    bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
+                    Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
+                    Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
+                    Adafruit_BMP280::FILTER_X16,      /* Filtering. */
+                    Adafruit_BMP280::STANDBY_MS_500); /* Standby time. */
+  } else {
+    DEBUG_PRINTLN("BMP280 sensor not found");
+  }  
+}
 
 // Initialize DHT sensor
 void initDHT() {

// Process serial communication using Modbus protocol
void communicate() {
#ifndef DEBUG
  // wait for the request 00 03 00 00 00 0A C4 1C
  boolean sendResponse = false;
  while (Serial.available() > 0) {
    byte inByte = (byte)Serial.read();
    
    if (inByte == 0x1C) {
      sendResponse = true;
    }
  }
  
  // send response
  if (sendResponse) {
    memcpy(modbusBuf + 3, (byte*)&dhtH, 4);
    memcpy(modbusBuf + 7, (byte*)&dhtT, 4);
    memcpy(modbusBuf + 11, (byte*)&dsT, 12);
    unsigned short crc = CRC16(modbusBuf, MODBUS_BUF_LEN - 2);
    memcpy(modbusBuf + 23, (byte*)&crc, 2);
    Serial.write(modbusBuf, MODBUS_BUF_LEN);
  }
#endif

  Serial.println(millis() - time);
}
#endif

// Read data from BMP280 sensor
void readBMP() {
  DEBUG_WRITETIME();
  DEBUG_PRINTLN("Reading from BMP280 sensor");

  if (bmpFound) {
    bmpP = bmp.readPressure();    // Pa
    bmpT = bmp.readTemperature(); // *C
  }
  
  DEBUG_WRITETIME();
  DEBUG_PRINT("t = ");
  DEBUG_PRINTLN(bmpT);
  DEBUG_PRINT("p = ");
  DEBUG_PRINTLN(bmpP);
  
  DEBUG_WRITETIME();
  DEBUG_PRINTLN();
}

// Read data from DHT sensor
void readDHT() {
