diff --git a/ampel-firmware.ino b/ampel-firmware.ino
index e1ff92e85d19c069bf74717fb7a43d5f807e9600..4465ae09caf27796a7667b6526ead446a829e706 100644
--- a/ampel-firmware.ino
+++ b/ampel-firmware.ino
@@ -64,7 +64,7 @@ void setup() {
 
   Serial.begin(BAUDS);
 
-  pinMode(0, INPUT);  // Flash button (used for forced calibration)
+  pinMode(0, INPUT); // Flash button (used for forced calibration)
 
   LedEffects::setupRing();
 
@@ -111,64 +111,12 @@ void loop() {
   //TODO: Restart every day or week, in order to not let t0 overflow?
   uint32_t t0 = millis();
 
-  /**
-   * USER INTERACTION
-   */
   keepServicesAlive();
+
   // Short press for night mode, Long press for calibration.
   checkFlashButton();
 
-  /**
-   * GET DATA
-   */
-  bool freshData = sensor::updateDataIfAvailable();
-
-  //NOTE: Data is available, but it's sometimes erroneous: the sensor outputs zero ppm but non-zero temperature and non-zero humidity.
-  if (sensor::co2 <= 0) {
-    // No measurement yet. Waiting.
-    LedEffects::showWaitingLED(color::blue);
-    return;
-  }
-
-  /**
-   * Fresh data. Show it and send it if needed.
-   */
-
-  if (freshData) {
-    sensor::timestamp = ntp::getLocalTime();
-    Serial.println(sensor::timestamp);
-
-    Serial.print(F("co2(ppm): "));
-    Serial.print(sensor::co2);
-    Serial.print(F(" temp(C): "));
-    Serial.print(sensor::temperature);
-    Serial.print(F(" humidity(%): "));
-    Serial.println(sensor::humidity);
-
-    csv_writer::logIfTimeHasCome(sensor::timestamp, sensor::co2, sensor::temperature, sensor::humidity);
-
-#ifdef MQTT
-    mqtt::publishIfTimeHasCome(sensor::timestamp, sensor::co2, sensor::temperature, sensor::humidity);
-#endif
-  }
-
-  if (sensor::co2 < 250) {
-    // Sensor should be calibrated.
-    LedEffects::showWaitingLED(color::magenta);
-    return;
-  }
-
-  /**
-   * Display data, even if it's "old" (with breathing).
-   * Those effects include a short delay.
-   */
-
-  if (sensor::co2 < 2000) {
-    LedEffects::displayCO2color(sensor::co2);
-    LedEffects::breathe(sensor::co2);
-  } else {  // >= 2000: entire ring blinks red
-    LedEffects::redAlert();
-  }
+  sensor::getAndSendData();
 
   uint32_t duration = millis() - t0;
   if (duration > max_loop_duration) {
diff --git a/co2_sensor.cpp b/co2_sensor.cpp
index 724588bb26526e622b01867712441ee211e59afa..f13957352a052aa992bfa4899940cc32981691ac 100644
--- a/co2_sensor.cpp
+++ b/co2_sensor.cpp
@@ -21,6 +21,9 @@ namespace sensor {
   float temperature = 0;
   float humidity = 0;
   String timestamp = "";
+  int16_t stable_measurements = 0;
+  uint32_t waiting_color = color::blue;
+  bool should_calibrate = false;
 
   void initialize() {
 #if defined(ESP8266)
@@ -69,7 +72,7 @@ namespace sensor {
 
   //NOTE: should time offset be used to reset measurement_timestep?
   void checkTimerDeviation() {
-    static int32_t previous_measurement_at = 0; // Used to monitor sensor timer offset.
+    static int32_t previous_measurement_at = 0;
     int32_t now = millis();
     Serial.print("Measurement time offset : ");
     Serial.print(now - previous_measurement_at - config::measurement_timestep * 1000);
@@ -77,9 +80,18 @@ namespace sensor {
     previous_measurement_at = now;
   }
 
-  bool updateDataIfAvailable() {
-    //TODO: Save count of stable measurements
+  void countStableMeasurements() {
     static int16_t previous_co2 = 0;
+    if (co2 > (previous_co2 - 30) && co2 < (previous_co2 + 30)) {
+      stable_measurements++;
+      waiting_color = color::green;
+    } else {
+      stable_measurements = 0;
+      waiting_color = color::red;
+    }
+  }
+
+  bool updateDataIfAvailable() {
     if (scd30.dataAvailable()) {
       // checkTimerDeviation();
       co2 = scd30.getCO2();
@@ -90,8 +102,7 @@ namespace sensor {
     return false;
   }
 
-  void waitUntilMeasurementsAreStable() {
-    //TODO: Refactor completely, in order to avoid very long loop?
+  void startCalibrationProcess() {
     /** From the sensor documentation:
      * For best results, the sensor has to be run in a stable environment in continuous mode at
      * a measurement rate of 2s for at least two minutes before applying the FRC command and sending the reference value.
@@ -100,41 +111,78 @@ namespace sensor {
     scd30.setMeasurementInterval(2); // [s] The change will only take effect after next measurement.
     Serial.println(F("Waiting until the measurements are stable for at least 2 minutes."));
     Serial.println(F("It could take a very long time."));
-
-    //########################################################################################################
-    // (c) Mario Lukas
-    // https://github.com/mariolukas/Watterott-CO2-Ampel-Plus-Firmware/blob/main/CO2-Ampel_Plus/Sensor.cpp#L57
-    uint32_t last_color = color::blue;
-    int stable_measurements = 0, last_co2 = 0;
-    for (stable_measurements = 0; stable_measurements < 60;) {
-      if (scd30.dataAvailable()) {
-        co2 = scd30.getCO2();
-        //No more than +/-30ppm variation compared to previous measurement.
-        if ((co2 > (last_co2 - 30)) && (co2 < (last_co2 + 30))) {
-          last_color = color::green;
-          stable_measurements++;
-        } else {
-          last_color = color::red;
-          stable_measurements = 0;
-        }
-        last_co2 = co2;
-      }
-      LedEffects::showKITTWheel(last_color, 1);
-    }
-    //########################################################################################################
+    should_calibrate = true;
   }
 
-  void startCalibrationProcess() {
-    waitUntilMeasurementsAreStable();
-    Serial.print("Starting SCD30 calibration...");
+  void calibrateAndRestart() {
+    Serial.print(F("Calibrating SCD30 now..."));
     scd30.setAltitudeCompensation(config::altitude_above_sea_level);
     scd30.setForcedRecalibrationFactor(config::co2_calibration_level);
-    Serial.println(" Done!");
-    Serial.println("Sensor calibrated.");
-    Serial.println("Sensor will now restart.");
+    Serial.println(F(" Done!"));
+    Serial.println(F("Sensor calibrated."));
+    Serial.println(F("Sensor will now restart."));
     LedEffects::showKITTWheel(color::green, 5);
     //TODO: Add LEDs off and move to util::reset()
     FS_LIB.end();
     ESP.restart();
   }
+
+  void getAndSendData() {
+    bool freshData = sensor::updateDataIfAvailable();
+
+    //NOTE: Data is available, but it's sometimes erroneous: the sensor outputs zero ppm but non-zero temperature and non-zero humidity.
+    if (sensor::co2 <= 0) {
+      // No measurement yet. Waiting.
+      LedEffects::showWaitingLED(color::blue);
+      return;
+    }
+
+    /**
+     * Fresh data. Show it and send it if needed.
+     */
+    if (freshData) {
+      countStableMeasurements();
+      sensor::timestamp = ntp::getLocalTime();
+      Serial.println(sensor::timestamp);
+
+      Serial.print(F("co2(ppm): "));
+      Serial.print(sensor::co2);
+      Serial.print(F(" temp(C): "));
+      Serial.print(sensor::temperature);
+      Serial.print(F(" humidity(%): "));
+      Serial.println(sensor::humidity);
+
+      csv_writer::logIfTimeHasCome(sensor::timestamp, sensor::co2, sensor::temperature, sensor::humidity);
+
+#ifdef MQTT
+      mqtt::publishIfTimeHasCome(sensor::timestamp, sensor::co2, sensor::temperature, sensor::humidity);
+#endif
+    }
+
+    if (should_calibrate) {
+      if (stable_measurements == 60) {
+        calibrateAndRestart();
+      }
+      LedEffects::showWaitingLED(waiting_color);
+      return;
+    }
+
+    if (sensor::co2 < 250) {
+      // Sensor should be calibrated.
+      LedEffects::showWaitingLED(color::magenta);
+      return;
+    }
+
+    /**
+     * Display data, even if it's "old" (with breathing).
+     * Those effects include a short delay.
+     */
+
+    if (sensor::co2 < 2000) {
+      LedEffects::displayCO2color(sensor::co2);
+      LedEffects::breathe(sensor::co2);
+    } else { // >= 2000: entire ring blinks red
+      LedEffects::redAlert();
+    }
+  }
 }
diff --git a/co2_sensor.h b/co2_sensor.h
index bd1e16febcecab8840fb919b05ff0c9aab6710fa..5c0871214151571f55c1683a0c62e78bcb01afd4 100644
--- a/co2_sensor.h
+++ b/co2_sensor.h
@@ -9,10 +9,14 @@
 #include "csv_writer.h" // To close filesystem before restart.
 #include <Wire.h>
 
+#ifdef MQTT
+#  include "mqtt.h"
+#endif
+
 namespace config {
-  extern uint16_t measurement_timestep;  // [s] Value between 2 and 1800 (range for SCD30 sensor)
+  extern uint16_t measurement_timestep; // [s] Value between 2 and 1800 (range for SCD30 sensor)
   extern const bool auto_calibrate_sensor; // [true / false]
-  extern uint16_t co2_calibration_level;  // [ppm]
+  extern uint16_t co2_calibration_level; // [ppm]
   extern const float temperature_offset; // [K] Sign isn't relevant.
 }
 
@@ -24,7 +28,7 @@ namespace sensor {
   extern String timestamp;
 
   void initialize();
-  bool updateDataIfAvailable();
+  void getAndSendData();
   void startCalibrationProcess();
 }
 #endif