diff --git a/ampel-firmware.ino b/ampel-firmware.ino
index 4365f368a3487983b50bc9d5626e30b012438865..adcd5b98098670f300bddad1a4ac891f84bdc299 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,73 +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::scd30.dataAvailable(); // Alternative : close to time-step AND dataAvailable, to avoid asking the sensor too often.
-
-  if (freshData) {
-    //TODO: Move to co2_sensor.cpp
-    //TODO: Save count of stable measurements
-    //TODO: Compare time to previous measurements, check that it's not too far away from config::measurement_interval
-    sensor::co2 = sensor::scd30.getCO2();
-    sensor::temperature = sensor::scd30.getTemperature();
-    sensor::humidity = sensor::scd30.getHumidity();
-  }
-
-  //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::processData();
 
   uint32_t duration = millis() - t0;
   if (duration > max_loop_duration) {
diff --git a/co2_sensor.cpp b/co2_sensor.cpp
index 68e6c0f1ad278afbbc5252318b557aa807d20d10..14946075e267cf66a3fb48dc6865fcb571ee25b9 100644
--- a/co2_sensor.cpp
+++ b/co2_sensor.cpp
@@ -2,17 +2,17 @@
 
 namespace config {
   // Values should be defined in config.h
-  uint16_t measurement_timestep = MEASUREMENT_TIMESTEP;  // [s] Value between 2 and 1800 (range for SCD30 sensor)
-  const uint16_t altitude_above_sea_level = ALTITUDE_ABOVE_SEA_LEVEL;  // [m]
-  uint16_t co2_calibration_level = ATMOSPHERIC_CO2_CONCENTRATION;  // [ppm]
+  uint16_t measurement_timestep = MEASUREMENT_TIMESTEP; // [s] Value between 2 and 1800 (range for SCD30 sensor)
+  const uint16_t altitude_above_sea_level = ALTITUDE_ABOVE_SEA_LEVEL; // [m]
+  uint16_t co2_calibration_level = ATMOSPHERIC_CO2_CONCENTRATION; // [ppm]
 #ifdef TEMPERATURE_OFFSET
   // Residual heat from CO2 sensor seems to be high enough to change the temperature reading. How much should it be offset?
   // NOTE: Sign isn't relevant. The returned temperature will always be shifted down.
-  const float temperature_offset = TEMPERATURE_OFFSET;  // [K]
+  const float temperature_offset = TEMPERATURE_OFFSET; // [K]
 #else
   const float temperature_offset = -3.0;  // [K] Temperature measured by sensor is usually at least 3K too high.
 #endif
-  const bool auto_calibrate_sensor = AUTO_CALIBRATE_SENSOR;  // [true / false]
+  const bool auto_calibrate_sensor = AUTO_CALIBRATE_SENSOR; // [true / false]
 }
 
 namespace sensor {
@@ -21,13 +21,16 @@ 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)
     Wire.begin(12, 14);  // ESP8266 - D6, D5;
 #endif
 #if defined(ESP32)
-    Wire.begin(21, 22);  // ESP32
+    Wire.begin(21, 22); // ESP32
     /**
      *  SCD30   ESP32
      *  VCC --- 3V3
@@ -46,11 +49,12 @@ namespace sensor {
     }
 
     // SCD30 has its own timer.
+    //NOTE: The timer seems to be inaccurate, though, possibly depending on voltage. Should it be offset?
     Serial.println();
     Serial.print(F("Setting SCD30 timestep to "));
     Serial.print(config::measurement_timestep);
     Serial.println(" s.");
-    scd30.setMeasurementInterval(config::measurement_timestep);  // [s]
+    scd30.setMeasurementInterval(config::measurement_timestep); // [s]
 
     Serial.print(F("Setting temperature offset to -"));
     Serial.print(abs(config::temperature_offset));
@@ -66,8 +70,43 @@ namespace sensor {
     Serial.println(config::auto_calibrate_sensor ? "ON." : "OFF.");
   }
 
-  void waitUntilMeasurementsAreStable() {
-    //TODO: Refactor completely, in order to avoid very long loop?
+  //NOTE: should timer deviation be used to adjust measurement_timestep?
+  void checkTimerDeviation() {
+    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);
+    Serial.println(" ms.");
+    previous_measurement_at = now;
+  }
+
+  void countStableMeasurements() {
+    static int16_t previous_co2 = 0;
+    if (co2 > (previous_co2 - 30) && co2 < (previous_co2 + 30)) {
+      stable_measurements++;
+      Serial.print(F("Number of stable measurements : "));
+      Serial.println(stable_measurements);
+      waiting_color = color::green;
+    } else {
+      stable_measurements = 0;
+      waiting_color = color::red;
+    }
+    previous_co2 = co2;
+  }
+
+  bool updateDataIfAvailable() {
+    if (scd30.dataAvailable()) {
+      // checkTimerDeviation();
+      timestamp = ntp::getLocalTime();
+      co2 = scd30.getCO2();
+      temperature = scd30.getTemperature();
+      humidity = scd30.getHumidity();
+      return true;
+    }
+    return false;
+  }
+
+  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.
@@ -76,41 +115,81 @@ 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.");
-    LedEffects::showKITTWheel(color::green, 5);
-    //TODO: Add LEDs off and move to util::reset()
-    FS_LIB.end();
-    ESP.restart();
+    Serial.println(F(" Done!"));
+    Serial.println(F("Sensor calibrated."));
+    resetAmpel();
+  }
+
+  void logToSerial() {
+    Serial.println(timestamp);
+    Serial.print(F("co2(ppm): "));
+    Serial.print(co2);
+    Serial.print(F(" temp(C): "));
+    Serial.print(temperature);
+    Serial.print(F(" humidity(%): "));
+    Serial.println(humidity);
+  }
+
+  void displayCO2OnLedRing() {
+    if (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 (co2 < 2000) {
+      LedEffects::displayCO2color(co2);
+      LedEffects::breathe(co2);
+    } else {
+      // >= 2000: entire ring blinks red
+      LedEffects::redAlert();
+    }
+  }
+
+  void processData() {
+    bool freshData = updateDataIfAvailable();
+
+    //NOTE: Data is available, but it's sometimes erroneous: the sensor outputs zero ppm but non-zero temperature and non-zero humidity.
+    if (co2 <= 0) {
+      // No measurement yet. Waiting.
+      LedEffects::showWaitingLED(color::blue);
+      return;
+    }
+
+    /**
+     * Fresh data. Log it and send it if needed.
+     */
+    if (freshData) {
+      if (should_calibrate) {
+        countStableMeasurements();
+      }
+
+      logToSerial();
+      csv_writer::logIfTimeHasCome(timestamp, co2, temperature, humidity);
+
+#ifdef MQTT
+      mqtt::publishIfTimeHasCome(timestamp, co2, temperature, humidity);
+#endif
+    }
+
+    if (should_calibrate) {
+      if (stable_measurements == 60) {
+        calibrateAndRestart();
+      }
+      LedEffects::showWaitingLED(waiting_color);
+      return;
+    }
+
+    displayCO2OnLedRing();
   }
 }
diff --git a/co2_sensor.h b/co2_sensor.h
index 97ea97f2c8b7762c7bab0470d293a558ad854481..624caa138f2a849d0254782bed8c0a6b35ba0258 100644
--- a/co2_sensor.h
+++ b/co2_sensor.h
@@ -6,13 +6,18 @@
 #include "src/lib/SparkFun_SCD30_Arduino_Library/src/SparkFun_SCD30_Arduino_Library.h"  // From: http://librarymanager/All#SparkFun_SCD30
 #include "config.h"
 #include "led_effects.h"
+#include "util.h"
 #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,6 +29,7 @@ namespace sensor {
   extern String timestamp;
 
   void initialize();
+  void processData();
   void startCalibrationProcess();
 }
 #endif
diff --git a/led_effects.cpp b/led_effects.cpp
index b2f51bfe5935107bce838b1d9f67e63fd1fe764c..3804ee1111cd21cd4e4c5a73b900dec772d046e6 100644
--- a/led_effects.cpp
+++ b/led_effects.cpp
@@ -11,7 +11,7 @@ namespace config {
 /*****************************************************************
  * Configuration  (calculated from above values)                 *
  *****************************************************************/
-namespace config  //TODO: Use a class instead. NightMode could then be another state.
+namespace config //TODO: Use a class instead. NightMode could then be another state.
 {
   const float average_brightness = 0.5 * (config::max_brightness + config::min_brightness);
   const float brightness_amplitude = 0.5 * (config::max_brightness - config::min_brightness);
@@ -34,12 +34,6 @@ const uint16_t CO2_TICKS[NUMPIXELS + 1] = { 0, 500, 600, 700, 800, 900, 1000, 12
 const uint16_t LED_HUES[NUMPIXELS] = { 21845, 19114, 16383, 13653, 10922, 8191, 5461, 2730, 0, 0, 0, 0 }; // [hue angle]
 Adafruit_NeoPixel pixels(NUMPIXELS, NEOPIXELS_PIN, NEO_GRB + NEO_KHZ800);
 
-namespace counter {
-  uint16_t wheel_offset = 0;
-  uint16_t kitt_offset = 0;
-  uint16_t breathing_offset = 0;
-}  // namespace counter
-
 namespace LedEffects {
   //On-board LED on D4, aka GPIO02
   const int ONBOARD_LED_PIN = 2;
@@ -56,18 +50,23 @@ namespace LedEffects {
     digitalWrite(ONBOARD_LED_PIN, LOW);
   }
 
+  void LEDsOff() {
+    pixels.clear();
+    pixels.show();
+    onBoardLEDOff();
+  }
+
   void setupRing() {
     pixels.begin();
     pixels.setBrightness(config::max_brightness);
-    pixels.clear();
+    LEDsOff();
   }
 
   void toggleNightMode() {
     config::night_mode = !config::night_mode;
     if (config::night_mode) {
       Serial.println(F("NIGHT MODE!"));
-      pixels.clear();
-      pixels.show();
+      LEDsOff();
     } else {
       Serial.println(F("DAY MODE!"));
     }
@@ -79,13 +78,14 @@ namespace LedEffects {
     if (config::night_mode) {
       return;
     }
+    static uint16_t kitt_offset = 0;
     pixels.clear();
     for (int j = config::kitt_tail; j >= 0; j--) {
-      int ledNumber = abs((counter::kitt_offset - j + NUMPIXELS) % (2 * NUMPIXELS) - NUMPIXELS) % NUMPIXELS; // Triangular function
+      int ledNumber = abs((kitt_offset - j + NUMPIXELS) % (2 * NUMPIXELS) - NUMPIXELS) % NUMPIXELS; // Triangular function
       pixels.setPixelColor(ledNumber, color * pixels.gamma8(255 - j * 76) / 255);
     }
     pixels.show();
-    counter::kitt_offset += 1;
+    kitt_offset++;
   }
 
   // Start K.I.T.T. led effect. Red color as default.
@@ -135,12 +135,13 @@ namespace LedEffects {
     if (config::night_mode) {
       return;
     }
+    static uint16_t wheel_offset = 0;
     unsigned long t0 = seconds();
     pixels.setBrightness(config::max_brightness);
     while (seconds() < t0 + duration_s) {
       for (int i = 0; i < NUMPIXELS; i++) {
-        pixels.setPixelColor(i, pixels.ColorHSV(i * 65535 / NUMPIXELS + counter::wheel_offset));
-        counter::wheel_offset += hue_increment;
+        pixels.setPixelColor(i, pixels.ColorHSV(i * 65535 / NUMPIXELS + wheel_offset));
+        wheel_offset += hue_increment;
       }
       pixels.show();
       delay(10);
@@ -165,14 +166,14 @@ namespace LedEffects {
 
   void breathe(int16_t co2) {
     if (!config::night_mode) {
+      static uint16_t breathing_offset = 0;
       //TODO: use integer sine
       pixels.setBrightness(
-          static_cast<int>(config::average_brightness
-              + cos(counter::breathing_offset * 0.1) * config::brightness_amplitude));
+          static_cast<int>(config::average_brightness + cos(breathing_offset * 0.1) * config::brightness_amplitude));
       pixels.show();
-      counter::breathing_offset += 1;
+      breathing_offset++;
     }
-    delay(co2 > 1600 ? 50 : 100);  // faster breathing for higher CO2 values
+    delay(co2 > 1600 ? 50 : 100); // faster breathing for higher CO2 values
   }
 
   /**
@@ -181,7 +182,7 @@ namespace LedEffects {
    */
   int countdownToZero() {
     if (config::night_mode) {
-      Serial.println("Night mode. Not doing anything.");
+      Serial.println(F("Night mode. Not doing anything."));
       delay(1000); // Wait for a while, to avoid coming back to this function too many times when button is pressed.
       return 1;
     }
diff --git a/led_effects.h b/led_effects.h
index a8d5b590dbe2bfb8312d01982114bee7c641854f..31368a2dea2842c4f2d3f884f71a2533d9e9a251 100644
--- a/led_effects.h
+++ b/led_effects.h
@@ -22,6 +22,7 @@ namespace LedEffects {
   void onBoardLEDOff();
   void onBoardLEDOn();
   void toggleNightMode();
+  void LEDsOff();
 
   void setupRing();
   void redAlert();
diff --git a/mqtt.cpp b/mqtt.cpp
index 46955483c1f556cf594ec80e93938d4595ca424c..ff8808b19277b8473738e9a087223180132a5ea2 100644
--- a/mqtt.cpp
+++ b/mqtt.cpp
@@ -162,8 +162,7 @@ namespace mqtt {
     } else if (messageString == "local_ip") {
       sendInfoAboutLocalNetwork();
     } else if (messageString == "reset") {
-      FS_LIB.end();
-      ESP.restart();
+      resetAmpel();
     } else {
       LedEffects::showKITTWheel(color::red, 1);
       Serial.println(F("Message not supported. Doing nothing."));
diff --git a/util.cpp b/util.cpp
index 31df19dd856e5c3f17b5eb929979660eb27db715..cd7c53bd1423d8ae3056711b9474f4cb1f65c823 100644
--- a/util.cpp
+++ b/util.cpp
@@ -38,6 +38,14 @@ namespace ntp {
   }
 }
 
+void resetAmpel() {
+  Serial.print("Resetting");
+  FS_LIB.end();
+  LedEffects::LEDsOff();
+  delay(1000);
+  ESP.restart();
+}
+
 uint32_t max_loop_duration = 0;
 
 //FIXME: Remove every instance of Strings, to avoid heap fragmentation problems. (Start:  "Free heap space : 17104 bytes")
diff --git a/util.h b/util.h
index 140ff672a6093985a246d64c7c2f1195aee1a288..4e0b64eebd70fba2af14d65ad867d52501826d61 100644
--- a/util.h
+++ b/util.h
@@ -3,6 +3,7 @@
 #include <Arduino.h>
 #include "config.h"
 #include "wifi_util.h" // To get MAC
+#include "csv_writer.h" // To close filesystem before reset
 
 #include <WiFiUdp.h> //required for NTP
 #include "src/lib/NTPClient-master/NTPClient.h" // NTP
@@ -27,4 +28,6 @@ namespace ntp {
 extern uint32_t max_loop_duration;
 const extern String SENSOR_ID;
 
+void resetAmpel();
+
 #endif
diff --git a/web_server.cpp b/web_server.cpp
index 6324aff164db8775d3ff6e55bbb0f39fe1fcea88..8b8025163a2de873bae58f04682a639b5d53423b 100644
--- a/web_server.cpp
+++ b/web_server.cpp
@@ -88,7 +88,7 @@ namespace web_server {
             "<tr><td>Last MQTT publish</td><td>%s</td></tr>\n"
             "<tr><td>MQTT publish timestep</td><td>%5d s</td></tr>\n"
 #endif
-            "<tr><td>Temperature offset</td><td>%.1fK</td></tr>\n"
+            "<tr><td>Temperature offset</td><td>%.1fK</td></tr>\n" //TODO: Read it from sensor?
             "<tr><td>Local address</td><td><a href='http://%s.local/'>%s.local</a></td></tr>\n"
             "<tr><td>Local IP</td><td><a href='http://%s'>%s</a></td></tr>\n"
             "<tr><td>Free heap space</td><td>%6d bytes</td></tr>\n"