diff --git a/platformio.ini b/platformio.ini
index 3bc685a0..053fa859 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -38,7 +38,11 @@ src_dir = src
 extra_configs = custom_config.ini
 
 [env:esp32dev]
-platform = espressif32 @ 5.3.0
+build_type = release
+platform = espressif32@5.4.0
+platform_packages =
+    toolchain-xtensa-esp32@12.2.0+20230208
+    framework-arduinoespressif32@https://github.com/espressif/arduino-esp32#2.0.11
 board = esp32dev
 framework = arduino
 monitor_speed = 115200
@@ -66,3 +70,12 @@ build_flags =
     -DHTTPS_CONNECTION_DATA_CHUNK_SIZE=1024
     ; build number "-dev" will be replaced in github action
     -DBUILD_NUMBER=\"-dev\"
+    -Wall
+    -std=gnu++20
+    -fno-rtti
+build_unflags =
+	-std=gnu++11
+debug_build_flags =
+    -Os
+debug_build_unflags =
+    -Og
diff --git a/src/OpenBikeSensorFirmware.cpp b/src/OpenBikeSensorFirmware.cpp
index a454563a..20d108d0 100644
--- a/src/OpenBikeSensorFirmware.cpp
+++ b/src/OpenBikeSensorFirmware.cpp
@@ -58,13 +58,17 @@ Button button(PUSHBUTTON_PIN);
 Config config;
 
 SSD1306DisplayDevice* obsDisplay;
-HCSR04SensorManager* sensorManager;
+DistanceSensorManager* sensorManager;
+#ifdef OBS_BLUETOOTH
 static BluetoothManager* bluetoothManager;
+#endif
 
 Gps gps;
 
+#ifdef OBS_BLUETOOTH
 static const long BLUETOOTH_INTERVAL_MILLIS = 100;
 static long lastBluetoothInterval = 0;
+#endif
 
 static const long DISPLAY_INTERVAL_MILLIS = 25;
 static long lastDisplayInterval = 0;
@@ -104,7 +108,6 @@ uint8_t batteryPercentage();
 void serverLoop();
 void handleButtonInServerMode();
 bool loadConfig(ObsConfig &cfg);
-void copyCollectedSensorData(DataSet *set);
 
 // The BMP280 can keep up to 3.4MHz I2C speed, so no need for an individual slower speed
 void switch_wire_speed_to_VL53(){
@@ -114,26 +117,14 @@ void switch_wire_speed_to_SSD1306(){
 	Wire.setClock(500000);
 }
 
+// FIXME
 void setupSensors() {
-  sensorManager = new HCSR04SensorManager;
-
-  HCSR04SensorInfo sensorManaged1;
-  sensorManaged1.triggerPin = (config.displayConfig & DisplaySwapSensors) ? 25 : 15;
-  sensorManaged1.echoPin = (config.displayConfig & DisplaySwapSensors) ? 26 : 4;
-  sensorManaged1.sensorLocation = (char*) "Right"; // TODO
-  sensorManager->registerSensor(sensorManaged1, 0);
-
-  HCSR04SensorInfo sensorManaged2;
-  sensorManaged2.triggerPin = (config.displayConfig & DisplaySwapSensors) ? 15 : 25;
-  sensorManaged2.echoPin = (config.displayConfig & DisplaySwapSensors) ? 4 : 26;
-  sensorManaged2.sensorLocation = (char*) "Left"; // TODO
-  sensorManager->registerSensor(sensorManaged2, 1);
-
+  sensorManager = new DistanceSensorManager;
   sensorManager->setOffsets(config.sensorOffsets);
-
-  sensorManager->setPrimarySensor(LEFT_SENSOR_ID);
+  sensorManager->initSensors();
 }
 
+#ifdef OBS_BLUETOOTH
 static void setupBluetooth(const ObsConfig &cfg, const String &trackUniqueIdentifier) {
   if (cfg.getProperty<bool>(ObsConfig::PROPERTY_BLUETOOTH)) {
     obsDisplay->showTextOnGrid(2, obsDisplay->newLine(), "Bluetooth ..");
@@ -193,6 +184,7 @@ static void buttonBluetooth(const DataSet *dataSet, uint16_t measureIndex) {
       left, right);
   }
 }
+#endif
 
 void setup() {
   Serial.begin(115200);
@@ -261,12 +253,6 @@ void setup() {
   }
   delay(333); // Added for user experience
 
-  //##############################################################
-  // Init HCSR04
-  //##############################################################
-
-  setupSensors();
-
   //##############################################################
   // Check, if the button is pressed
   // Enter configuration mode and enable OTA
@@ -283,8 +269,10 @@ void setup() {
 
   if (button.read() == HIGH || triggerServerMode) {
     obsDisplay->showTextOnGrid(2, obsDisplay->newLine(), "Start Server");
+#ifdef OBS_BLUETOOTH
     ESP_ERROR_CHECK_WITHOUT_ABORT(
       esp_bt_mem_release(ESP_BT_MODE_BTDM)); // no bluetooth at all here.
+#endif
 
     delay(200);
     startServer(&cfg);
@@ -333,7 +321,9 @@ void setup() {
 
   gps.handle();
 
+#ifdef OBS_BLUETOOTH
   setupBluetooth(cfg, trackUniqueIdentifier);
+#endif
 
   obsDisplay->showTextOnGrid(2, obsDisplay->newLine(), "Wait for GPS");
   obsDisplay->newLine();
@@ -343,8 +333,6 @@ void setup() {
   while (!gps.hasFix(obsDisplay)) {
     currentTimeMillis = millis();
     gps.handle();
-    sensorManager->pollDistancesAlternating();
-    reportBluetooth();
     gps.showWaitStatus(obsDisplay);
     if (button.read() == HIGH) {
       log_d("Skipped get GPS...");
@@ -360,12 +348,16 @@ void setup() {
   gps.handle(1100); // Added for user experience
   gps.pollStatistics();
   obsDisplay->clear();
+
+  //##############################################################
+  // Init HCSR04
+  //##############################################################
+  setupSensors();
 }
 
 void serverLoop() {
   gps.handle();
   configServerHandle();
-  sensorManager->pollDistancesAlternating();
   handleButtonInServerMode();
 }
 
@@ -426,7 +418,7 @@ void loop() {
   currentSet->millis = startTimeMillis;
   currentSet->batteryLevel = voltageMeter->read();
 
-  lastMeasurements = sensorManager->m_sensors[confirmationSensorID].numberOfTriggers;
+  lastMeasurements = sensorManager->getSensor(confirmationSensorID)->numberOfTriggers;
   sensorManager->reset(startTimeMillis);
 
   // if the detected minimum was measured more than 5s ago, it is discarded and cannot be confirmed
@@ -449,18 +441,11 @@ void loop() {
     }
     button.handle(currentTimeMillis);
     gps.handle();
-    if (sensorManager->pollDistancesAlternating()) {
+    if (sensorManager->pollDistances()) {
       // if a new minimum on the selected sensor is detected, the value and the time of detection will be stored
-      const uint16_t reading = sensorManager->sensorValues[confirmationSensorID];
+      const uint16_t reading = sensorManager->getLastValidRawDistance(confirmationSensorID);
       if (reading > 0 && reading < minDistanceToConfirm) {
         minDistanceToConfirm = reading;
-        minDistanceToConfirmIndex = sensorManager->getCurrentMeasureIndex();
-        // if there was no measurement of this sensor for this index, it is the
-        // one before. This happens with fast confirmations.
-        while (minDistanceToConfirmIndex > 0
-               && sensorManager->m_sensors[confirmationSensorID].echoDurationMicroseconds[minDistanceToConfirmIndex] <= 0) {
-            minDistanceToConfirmIndex--;
-        }
         datasetToConfirm = currentSet;
         timeOfMinimum = currentTimeMillis;
       }
@@ -469,8 +454,8 @@ void loop() {
     if (lastDisplayInterval != (currentTimeMillis / DISPLAY_INTERVAL_MILLIS)) {
       lastDisplayInterval = currentTimeMillis / DISPLAY_INTERVAL_MILLIS;
       obsDisplay->showValues(
-        sensorManager->m_sensors[LEFT_SENSOR_ID],
-        sensorManager->m_sensors[RIGHT_SENSOR_ID],
+        sensorManager->getSensor(LEFT_SENSOR_ID),
+        sensorManager->getSensor(RIGHT_SENSOR_ID),
         minDistanceToConfirm,
         voltageMeter->readPercentage(),
         (int16_t) TemperatureValue,
@@ -481,7 +466,9 @@ void loop() {
       );
     }
     gps.handle();
+#ifdef OBS_BLUETOOTH
     reportBluetooth();
+#endif
     if (button.gotPressed()) { // after button was released, detect long press here
       // immediate user feedback - we start the action
       obsDisplay->highlight();
@@ -491,13 +478,18 @@ void loop() {
       if (datasetToConfirm != nullptr) {
         datasetToConfirm->confirmedDistances.push_back(minDistanceToConfirm);
         datasetToConfirm->confirmedDistancesIndex.push_back(minDistanceToConfirmIndex);
+#ifdef OBS_BLUETOOTH
         buttonBluetooth(datasetToConfirm, minDistanceToConfirmIndex);
+#endif
         datasetToConfirm = nullptr;
       } else { // confirming an overtake without left measure
-        currentSet->confirmedDistances.push_back(MAX_SENSOR_VALUE);
-        currentSet->confirmedDistancesIndex.push_back(
-          sensorManager->getCurrentMeasureIndex());
+        if (sensorManager->hasReadings(LEFT_SENSOR_ID)) {
+          currentSet->confirmedDistances.push_back(MAX_SENSOR_VALUE);
+          currentSet->confirmedDistancesIndex.push_back(0);
+        }
+#ifdef OBS_BLUETOOTH
         buttonBluetooth(currentSet, sensorManager->getCurrentMeasureIndex());
+#endif
       }
       minDistanceToConfirm = MAX_SENSOR_VALUE; // ready for next confirmation
     }
@@ -510,7 +502,7 @@ void loop() {
   currentSet->gpsRecord = gps.getCurrentGpsRecord();
   currentSet->isInsidePrivacyArea = gps.isInsidePrivacyArea();
   if (currentSet->gpsRecord.getTow() == thisLoopTow) {
-    copyCollectedSensorData(currentSet);
+    sensorManager->copyData(currentSet);
     log_d("NEW SET: TOW: %u GPSms: %u, SETms: %u, GPS Time: %s, SET Time: %s, innerLoops: %d, buffer: %d, write time %3ums, loop time: %4ums, measurements: %2d",
           currentSet->gpsRecord.getTow(), currentSet->gpsRecord.getCreatedAtMillisTicks(),
           currentSet->millis,
@@ -561,19 +553,6 @@ void loop() {
         TimeUtils::timeToString().c_str(), thisLoopTow);
 }
 
-void copyCollectedSensorData(DataSet *set) {// Write the minimum values of the while-loop to a set
-  for (auto & m_sensor : sensorManager->m_sensors) {
-    set->sensorValues.push_back(m_sensor.minDistance);
-  }
-  set->measurements = sensorManager->lastReadingCount;
-  memcpy(&(set->readDurationsRightInMicroseconds),
-         &(sensorManager->m_sensors[0].echoDurationMicroseconds), set->measurements * sizeof(int32_t));
-  memcpy(&(set->readDurationsLeftInMicroseconds),
-         &(sensorManager->m_sensors[1].echoDurationMicroseconds), set->measurements * sizeof(int32_t));
-  memcpy(&(set->startOffsetMilliseconds),
-         &(sensorManager->startOffsetMilliseconds), set->measurements * sizeof(uint16_t));
-}
-
 uint8_t batteryPercentage() {
   uint8_t result = 0;
   if (voltageMeter) {
diff --git a/src/config.h b/src/config.h
index da970b9c..537e1b73 100644
--- a/src/config.h
+++ b/src/config.h
@@ -29,6 +29,10 @@
 #include <vector>
 #include <FS.h>
 
+#include <string>
+
+typedef std::basic_string<char> obs_string;
+
 enum DisplayOptions {
   DisplaySatellites = 0x01,  // 1
   DisplayVelocity = 0x02,   // 2
diff --git a/src/configServer.cpp b/src/configServer.cpp
index 2c982048..ab494792 100644
--- a/src/configServer.cpp
+++ b/src/configServer.cpp
@@ -419,7 +419,7 @@ static String getParameter(const std::vector<std::pair<String,String>> &params,
 
 
 static String getParameter(HTTPRequest *req, const String& name, const String&  def = "") {
-  std::string value;
+  obs_string value;
   if (req->getParams()->getQueryParameter(name.c_str(), value)) {
     return String(value.c_str());
   }
@@ -557,7 +557,7 @@ void registerPages(HTTPServer * httpServer) {
   httpServer->registerNode(new ResourceNode("/settings/security", HTTP_POST, handleSettingSecurityAction));
 
   httpServer->addMiddleware(&accessFilter);
-  httpServer->setDefaultHeader("Server", std::string("OBS/") + OBSVersion);
+  httpServer->setDefaultHeader("Server", obs_string("OBS/") + OBSVersion);
 }
 
 void beginPages() {
@@ -566,7 +566,7 @@ void beginPages() {
   insecureServer->registerNode(new ResourceNode("/cert", HTTP_GET, handleDownloadCert));
   insecureServer->registerNode(new ResourceNode("/http", HTTP_POST, handleHttpAction));
   insecureServer->setDefaultNode(new ResourceNode("", HTTP_GET,  handleHttpsRedirect));
-  insecureServer->setDefaultHeader("Server", std::string("OBS/") + OBSVersion);
+  insecureServer->setDefaultHeader("Server", obs_string("OBS/") + OBSVersion);
 }
 
 static int ticks;
@@ -702,7 +702,7 @@ static void wifiConectedActions() {
 }
 
 /* callback function called if wifi data is received via improv */
-bool initWifi(const std::string & ssid, const std::string & password) {
+bool initWifi(const obs_string & ssid, const obs_string & password) {
   log_i("Received WiFi credentials for SSID '%s'", ssid.c_str());
   theObsConfig->setProperty(0, ObsConfig::PROPERTY_WIFI_SSID, ssid);
   theObsConfig->setProperty(0, ObsConfig::PROPERTY_WIFI_PASSWORD, password);
@@ -735,8 +735,8 @@ static ObsImprov::State improvCallbackGetWifiStatus() {
   return result;
 }
 
-static std::string improvCallbackGetDeviceUrl() {
-  std::string url = "";
+static obs_string improvCallbackGetDeviceUrl() {
+  obs_string url = "";
   if (WiFiClass::status() == WL_CONNECTED) {
     theObsConfig->saveConfig();
     url += "http://";
@@ -881,7 +881,7 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) {
 
   res->print("<h3>ESP32</h3>");
   res->print(keyValue("Chip Model", ESP.getChipModel()));
-  res->print(keyValue("Chip Revision", ESP.getChipRevision()));
+  res->print(keyValue("Chip Revision", (uint32_t) ESP.getChipRevision()));
   res->print(keyValue("Heap size", ObsUtils::toScaledByteString(ESP.getHeapSize())));
   res->print(keyValue("Free heap", ObsUtils::toScaledByteString(ESP.getFreeHeap())));
   res->print(keyValue("Min. free heap", ObsUtils::toScaledByteString(ESP.getMinFreeHeap())));
@@ -909,8 +909,8 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) {
   res->print(page);
   page.clear();
 
-  page += keyValue("Cores", ESP.getChipCores());
-  page += keyValue("CPU frequency", ESP.getCpuFreqMHz(), "MHz");
+  page += keyValue("Cores", (const uint32_t) ESP.getChipCores());
+  page += keyValue("CPU frequency", (const uint32_t) ESP.getCpuFreqMHz(), "MHz");
 
   page += keyValue("SPIFFS size", ObsUtils::toScaledByteString(SPIFFS.totalBytes()));
   page += keyValue("SPIFFS used", ObsUtils::toScaledByteString(SPIFFS.usedBytes()));
@@ -973,12 +973,12 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) {
   page += keyValue("SD fs used", ObsUtils::toScaledByteString(SD.usedBytes()));
 
   page += "<h3>TOF Sensors</h3>";
-  page += keyValue("Left Sensor raw", sensorManager->getRawMedianDistance(LEFT_SENSOR_ID), "cm");
+  page += keyValue("Left Sensor raw", (const uint32_t) sensorManager->getMedianRawDistance(LEFT_SENSOR_ID), "cm");
   page += keyValue("Left Sensor max duration", sensorManager->getMaxDurationUs(LEFT_SENSOR_ID), "&#xB5;s");
   page += keyValue("Left Sensor min duration", sensorManager->getMinDurationUs(LEFT_SENSOR_ID), "&#xB5;s");
   page += keyValue("Left Sensor last start delay", sensorManager->getLastDelayTillStartUs(LEFT_SENSOR_ID), "&#xB5;s");
   page += keyValue("Left Sensor signal errors", sensorManager->getNoSignalReadings(LEFT_SENSOR_ID));
-  page += keyValue("Right Sensor raw", sensorManager->getRawMedianDistance(RIGHT_SENSOR_ID), "cm");
+  page += keyValue("Right Sensor raw", (const uint32_t) sensorManager->getMedianRawDistance(RIGHT_SENSOR_ID), "cm");
   page += keyValue("Right Sensor max duration", sensorManager->getMaxDurationUs(RIGHT_SENSOR_ID), "&#xB5;s");
   page += keyValue("Right Sensor min duration", sensorManager->getMinDurationUs(RIGHT_SENSOR_ID), "&#xB5;s");
   page += keyValue("Right Sensor last start delay", sensorManager->getLastDelayTillStartUs(RIGHT_SENSOR_ID), "&#xB5;s");
@@ -993,18 +993,18 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) {
   page += keyValue("GPS hdop", gps.getCurrentGpsRecord().getHdopString());
   page += keyValue("GPS fix", String(gps.getCurrentGpsRecord().getFixStatus(), 16));
   page += keyValue("GPS fix flags", String(gps.getCurrentGpsRecord().getFixStatusFlags(), 16));
-  page += keyValue("GPS satellites", gps.getValidSatellites());
+  page += keyValue("GPS satellites",(const uint32_t) gps.getValidSatellites());
   page += keyValue("GPS uptime", gps.getUptime(), "ms");
-  page += keyValue("GPS noise level", gps.getLastNoiseLevel());
+  page += keyValue("GPS noise level", (const uint32_t) gps.getLastNoiseLevel());
   page += keyValue("GPS baud rate", gps.getBaudRate());
   page += keyValue("GPS ALP bytes", gps.getNumberOfAlpBytesSent());
   page += keyValue("GPS messages", gps.getMessagesHtml());
 
   page += "<h3>Display / Button</h3>";
-  page += keyValue("Button State", digitalRead(PUSHBUTTON_PIN));
-  page += keyValue("Display i2c last error", Wire.getWriteError());
+  page += keyValue("Button State",(const int32_t) digitalRead(PUSHBUTTON_PIN));
+  page += keyValue("Display i2c last error", (const uint32_t) Wire.getWriteError());
   page += keyValue("Display i2c speed", Wire.getClock() / 1000, "KHz");
-  page += keyValue("Display i2c timeout", Wire.getTimeOut(), "ms");
+  page += keyValue("Display i2c timeout",(const uint32_t) Wire.getTimeOut(), "ms");
 
   page += "<h3>WiFi</h3>";
   page += keyValue("Local IP", WiFi.localIP().toString());
@@ -1058,7 +1058,6 @@ static void handleBackupDownload(HTTPRequest *, HTTPResponse * res) {
 
 static void handleBackupRestore(HTTPRequest * req, HTTPResponse * res) {
   HTTPMultipartBodyParser parser(req);
-  sensorManager->detachInterrupts();
 
   while(parser.nextField()) {
 
@@ -1093,7 +1092,6 @@ static void handleBackupRestore(HTTPRequest * req, HTTPResponse * res) {
       res->print("ERROR");
     }
   }
-  sensorManager->attachInterrupts();
 }
 
 
@@ -1342,8 +1340,8 @@ void uploadTracks(HTTPResponse *res) {
   obsDisplay->showTextOnGrid(3, 5, String(failedCount));
   if (res) {
     html += "</div><h3>...all files done</h3/>";
-    html += keyValue("OK", okCount);
-    html += keyValue("Failed", failedCount);
+    html += keyValue("OK",(const uint32_t) okCount);
+    html += keyValue("Failed", (const uint32_t) failedCount);
     html += "<input type=button onclick=\"window.location.href='/'\" class='btn' value='OK' />";
     html += footer;
     res->print(html);
@@ -1533,7 +1531,6 @@ static void handleFlashUpdateUrlAction(HTTPRequest * req, HTTPResponse * res) {
   log_i("Flash App Url is '%s'", url.c_str());
 
   Firmware f(String("OBS/") + String(OBSVersion));
-  sensorManager->detachInterrupts();
   if (f.downloadToFlash(url, updateProgress)) {
     obsDisplay->showTextOnGrid(0, 3, "Success!");
     sendRedirect(res, "/updatesd");
@@ -1542,13 +1539,11 @@ static void handleFlashUpdateUrlAction(HTTPRequest * req, HTTPResponse * res) {
     obsDisplay->showTextOnGrid(0, 4, f.getLastMessage());
     sendRedirect(res, "/about");
   }
-  sensorManager->attachInterrupts();
 }
 
 static void handleFlashFileUpdateAction(HTTPRequest *req, HTTPResponse *res) {
   // TODO: Add some assertions, cleanup with handleFlashUpdateUrlAction
   HTTPMultipartBodyParser parser(req);
-  sensorManager->detachInterrupts();
   Update.begin();
   Update.onProgress([](size_t pos, size_t all) {
     obsDisplay->drawProgressBar(4, pos, all);
@@ -1585,7 +1580,6 @@ static void handleFlashFileUpdateAction(HTTPRequest *req, HTTPResponse *res) {
       res->print(errorMsg);
     }
   }
-  sensorManager->attachInterrupts();
 }
 
 static void handleDeleteFiles(HTTPRequest *req, HTTPResponse * res) {
@@ -1856,7 +1850,7 @@ static void accessFilter(HTTPRequest * req, HTTPResponse * res, std::function<vo
     res->setStatusCode(401);
     res->setStatusText("Unauthorized");
     res->setHeader("Content-Type", "text/plain");
-    res->setHeader("WWW-Authenticate", std::string("Basic realm=\"") + OBS_ID_SHORT.c_str() + "\"");
+    res->setHeader("WWW-Authenticate", obs_string("Basic realm=\"") + OBS_ID_SHORT.c_str() + "\"");
     res->println("401: See OBS display");
 
     obsDisplay->clearProgressBar(5);
diff --git a/src/displays.cpp b/src/displays.cpp
index c5a9db13..7d41fa58 100644
--- a/src/displays.cpp
+++ b/src/displays.cpp
@@ -55,13 +55,13 @@ void SSD1306DisplayDevice::displaySimple(uint16_t value) {
 }
 
 void SSD1306DisplayDevice::showValues(
-  HCSR04SensorInfo sensor1, HCSR04SensorInfo sensor2, uint16_t minDistanceToConfirm,  int16_t batteryPercentage,
+  DistanceSensor* sensor1, DistanceSensor* sensor2, uint16_t minDistanceToConfirm,  int16_t batteryPercentage,
   int16_t TemperaturValue, int lastMeasurements, boolean insidePrivacyArea,
   double speed, uint8_t satellites) {
 
   handleHighlight();
 
-  uint16_t value1 = sensor1.minDistance;
+  uint16_t value1 = sensor1->minDistance;
   if (minDistanceToConfirm != MAX_SENSOR_VALUE) {
     value1 = minDistanceToConfirm;
   }
@@ -69,7 +69,7 @@ void SSD1306DisplayDevice::showValues(
     displaySimple(value1);
   } else {
     if (config.displayConfig & DisplayLeft) {
-      String loc1 = sensor1.sensorLocation;
+      String loc1 = sensor1->sensorLocation;
       if (insidePrivacyArea) {
         loc1 = "(" + loc1 + ")";
       }
@@ -83,8 +83,8 @@ void SSD1306DisplayDevice::showValues(
     }
     // Show sensor2, when DisplayRight is configured
     if (config.displayConfig & DisplayRight) {
-      uint16_t value2 = sensor2.distance;
-      String loc2 = sensor2.sensorLocation;
+      uint16_t value2 = sensor2->distance;
+      String loc2 = sensor2->sensorLocation;
       this->prepareTextOnGrid(3, 0, loc2);
       if (value2 == MAX_SENSOR_VALUE || value2 == 0) {
         this->prepareTextOnGrid(2, 1, "---", LARGE_FONT, 5, 0);
@@ -98,8 +98,8 @@ void SSD1306DisplayDevice::showValues(
     const int bufSize = 64;
     char buffer[bufSize];
 // #ifdef NERD_SENSOR_DISTANCE
-    snprintf(buffer, bufSize - 1, "%03d|%02d|%03d", sensor1.rawDistance,
-             lastMeasurements, sensor2.rawDistance);
+    snprintf(buffer, bufSize - 1, "%03d|%02d|%03d", sensor1->rawDistance,
+             lastMeasurements, sensor2->rawDistance);
 // #endif
 #ifdef NERD_HEAP
     snprintf(buffer, bufSize - 1, "%03d|%02d|%uk", sensor1.rawDistance,
diff --git a/src/displays.h b/src/displays.h
index 60aadb8d..f81f02d0 100644
--- a/src/displays.h
+++ b/src/displays.h
@@ -51,7 +51,7 @@ extern const uint8_t TempLogo[];
 #define HUGE_FONT Open_Sans_Regular_Plain_50
 
 // Forward declare classes to build (because there is a cyclic dependency between sensor.h and displays.h)
-class HCSR04SensorInfo;
+class DistanceSensor;
 
 const int CLK = 33; //Set the CLK pin connection to the display
 const int DIO = 25; //Set the DIO pin connection to the display
@@ -317,7 +317,7 @@ class SSD1306DisplayDevice : public DisplayDevice {
     void showNumButtonPressed();
 
     void showValues(
-      HCSR04SensorInfo sensor1, HCSR04SensorInfo sensor2,
+      DistanceSensor* sensor1, DistanceSensor* sensor2,
       uint16_t minDistanceToConfirm, int16_t batteryPercentage, int16_t TemperaturValue,
       int lastMeasurements, boolean insidePrivacyArea, double speed, uint8_t satellites);
 
diff --git a/src/globals.h b/src/globals.h
index d430ed62..6eadccc8 100644
--- a/src/globals.h
+++ b/src/globals.h
@@ -37,6 +37,7 @@ class HCSR04SensorManager;
 #include "sensor.h"
 #include "VoltageMeter.h"
 
+
 // This file should contain declarations of all variables that will be used globally.
 // The variables don't have to be set here, but need to be declared.
 
@@ -54,7 +55,7 @@ extern Config config;
 
 extern SSD1306DisplayDevice* obsDisplay;
 
-extern HCSR04SensorManager* sensorManager;
+extern DistanceSensorManager* sensorManager;
 
 extern VoltageMeter* voltageMeter;
 
diff --git a/src/obsimprov.h b/src/obsimprov.h
index 0c857095..f5e5206a 100644
--- a/src/obsimprov.h
+++ b/src/obsimprov.h
@@ -33,6 +33,8 @@
 #include <functional>
 #include <vector>
 
+#include "config.h"
+
 /**
  * Utility class to implement the improv protocol.
  * See protocol documentation at https://www.improv-wifi.com/serial/ for
@@ -57,9 +59,9 @@ class ObsImprov {
      * @param getDeviceUrl if the device wifi is up the url needed to reach
      *                     the device should be returned, a empty string otherwise.
      */
-    ObsImprov(std::function<bool(const std::string & ssid, const std::string & password)> initWifi,
+    ObsImprov(std::function<bool(const obs_string & ssid, const obs_string & password)> initWifi,
               std::function<State()> getWifiStatus,
-              std::function<std::string()> getDeviceUrl,
+              std::function<obs_string()> getDeviceUrl,
                HardwareSerial* serial = &Serial) :
                 mSerial(serial),
                 mInitWifi(initWifi),
@@ -83,10 +85,10 @@ class ObsImprov {
      * Should be called as soon as possible after creating a ObsImprov instance.
      * Empty data is reported upstream if needed earlier.
      */
-    void setDeviceInfo(const std::string & firmwareName,
-                       const std::string & firmwareVersion,
-                       const std::string & hardwareVariant,
-                       const std::string & deviceName);
+    void setDeviceInfo(const obs_string & firmwareName,
+                       const obs_string & firmwareVersion,
+                       const obs_string & hardwareVariant,
+                       const obs_string & deviceName);
 
     /**
      * Returns true if any improv message was received.
@@ -124,21 +126,21 @@ class ObsImprov {
     HardwareSerial* mSerial;
     std::vector<uint8_t> mBuffer;
     uint8_t  mHeaderPos = 0;
-    std::string mFirmwareName;
-    std::string mFirmwareVersion;
-    std::string mHardwareVariant;
-    std::string mDeviceName;
+    obs_string mFirmwareName = "";
+    obs_string mFirmwareVersion = "";
+    obs_string mHardwareVariant = "";
+    obs_string mDeviceName = "";
     static const char *HEADER;
     static const uint8_t HEADER_LENGTH;
-    const std::function<bool(const std::string & ssid, const std::string & password)> mInitWifi;
+    const std::function<bool(const obs_string & ssid, const obs_string & password)> mInitWifi;
     const std::function<State()> mWifiStatus;
-    const std::function<std::string()> mDeviceUrl;
+    const std::function<obs_string()> mDeviceUrl;
     bool mImprovActive = false;
     void sendWifiSuccess(Command cmd = Command::WIFI_SETTINGS) const;
     void sendCurrentState(State state) const;
     void sendErrorState(Error error) const;
     void handleRpcGetDeviceInfo() const;
-    void appendStringAndLength(std::vector<uint8_t> &response, std::string data) const;
+    void appendStringAndLength(std::vector<uint8_t> &response, obs_string data) const;
     void sendPayload(Stream *stream, std::vector<uint8_t> payload) const;
     bool isCompleteImprovMessage(std::vector<uint8_t> buffer) const;
     bool isValidImprovMessage(std::vector<uint8_t> buffer) const;
diff --git a/src/sensor.cpp b/src/sensor.cpp
index faadaa1b..7cda1d46 100644
--- a/src/sensor.cpp
+++ b/src/sensor.cpp
@@ -22,6 +22,8 @@
  */
 
 #include "sensor.h"
+
+#include "writer.h"
 #include "FunctionalInterrupt.h"
 /*
  * Sensor types:
@@ -46,348 +48,186 @@ static const uint16_t MAX_DISTANCE_MEASURED_CM = 320; // candidate to check I co
 static const uint32_t MIN_DURATION_MICRO_SEC = MIN_DISTANCE_MEASURED_CM * MICRO_SEC_TO_CM_DIVIDER;
 const uint32_t MAX_DURATION_MICRO_SEC = MAX_DISTANCE_MEASURED_CM * MICRO_SEC_TO_CM_DIVIDER;
 
-/* This time is maximum echo pin high time if the sensor gets no response
- * HC-SR04: observed 71ms, docu 60ms
- * JSN-SR04T: observed 58ms
- */
-static const uint32_t MAX_TIMEOUT_MICRO_SEC = 75000;
-
-/* To avoid that we receive a echo from a former measurement, we do not
- * start a new measurement within the given time from the start of the
- * former measurement of the opposite sensor.
- * High values can lead to the situation that we only poll the
- * primary sensor for a while!?
- */
-static const uint32_t SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC = 30 * 1000;
-
-/* The last end (echo goes to low) of a measurement must be this far
- * away before a new measurement is started.
- */
-static const uint32_t SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC = 10 * 1000;
-
-/* The last start of a new measurement must be as long ago as given here
-    away, until we start a new measurement.
-   - With 35ms I could get stable readings down to 19cm (new sensor)
-   - With 30ms I could get stable readings down to 25/35cm only (new sensor)
-   - It looked fine with the old sensor for all values
- */
-static const uint32_t SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC = 35 * 1000;
-
-/* Value of HCSR04SensorInfo::end during an ongoing measurement. */
-static const uint32_t MEASUREMENT_IN_PROGRESS = 0;
-
-/* Some calculations:
- *
- * Assumption:
- *  Small car length: 4m (average 2017 4,4m, really smallest is 2.3m )
- *  Speed difference: 100km/h = 27.8m/s
- *  Max interesting distance: MAX_DISTANCE_MEASURED_CM = 250cm
- *
- * Measured:
- *  MAX_TIMEOUT_MICRO_SEC == ~ 75_000 micro seconds, if the sensor gets no echo
- *
- * Time of the overtake process:
- *  4m / 27.8m/s = 0.143 sec
- *
- * We need to have time for 3 measures to get the car
- *
- * 0.148 sec / 3 measures = 48 milli seconds pre measure = 48_000 micro_sec
- *
- * -> If we wait till MAX_TIMEOUT_MICRO_SEC (75ms) because the right
- *    sensor has no measure we are to slow. (3x75 = 225ms > 143ms)
- *
- *    If we only waif tor the primary (left) sensor we are good again,
- *    and so trigger one sensor while the other is still in "timeout-state"
- *    we save some time. Assuming 1st measure is just before the car appears
- *    besides the sensor:
- *
- *    75ms open sensor + 2x SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC (35ms) = 145ms which is
- *    close to the needed 148ms
- */
-
-// Hack to get the pointers to the isr
-static HCSR04SensorInfo * TOF_SENSOR[NUMBER_OF_TOF_SENSORS];
+void DistanceSensorManager::initSensors() {
+  m_sensors[0].sensorLocation = "Right";
+  m_sensors[1].sensorLocation = "Left";
 
-void HCSR04SensorManager::registerSensor(const HCSR04SensorInfo& sensorInfo, uint8_t idx) {
-  if (idx >= NUMBER_OF_TOF_SENSORS) {
-    log_e("Can not register sensor for index %d, only %d tof sensors supported", idx, NUMBER_OF_TOF_SENSORS);
-    return;
-  }
-  m_sensors[idx] = sensorInfo;
-  pinMode(sensorInfo.triggerPin, OUTPUT);
-  pinMode(sensorInfo.echoPin, INPUT_PULLUP); // hint from https://youtu.be/xwsT-e1D9OY?t=354
-  sensorValues[idx] = MAX_SENSOR_VALUE;
-  m_sensors[idx].median = new Median<uint16_t>(5, MAX_SENSOR_VALUE);
-
-  TOF_SENSOR[idx] = &m_sensors[idx];
-  attachSensorInterrupt(idx);
-}
-
-static void IRAM_ATTR isr(HCSR04SensorInfo* const sensor) {
-  // since the measurement of start and stop use the same interrupt
-  // mechanism we should see a similar delay.
-  if (sensor->end == MEASUREMENT_IN_PROGRESS) {
-    const uint32_t now = micros();
-    if (HIGH == digitalRead(sensor->echoPin)) {
-      sensor->start = now;
-    } else { // LOW
-      sensor->end = now;
-    }
-  }
-}
-
-static void IRAM_ATTR isr0() {
-  isr(TOF_SENSOR[0]);
-}
-
-static void IRAM_ATTR isr1() {
-  isr(TOF_SENSOR[1]);
-}
-
-void HCSR04SensorManager::attachSensorInterrupt(uint8_t idx) {
-  // bad bad bad ....
-  if (idx == 0) {
-    attachInterrupt(TOF_SENSOR[idx]->echoPin, isr0, CHANGE);
+  sensor_queue = xQueueCreate( 40, sizeof( LLMessage ));
+  if (sensor_queue == 0) {
+    log_e("Oh wow. Sensor queue was not created successfully :(");
   } else {
-    attachInterrupt(TOF_SENSOR[idx]->echoPin, isr1, CHANGE);
+    log_i("Sensor queue created");
   }
 
-  // a solution like below leads to crashes with:
-  // Guru Meditation Error: Core  1 panic'ed (Cache disabled but cached memory region accessed)
-  // Core 1 was running in ISR context:
-  //   attachInterrupt(sensorInfo.echoPin,
-  //                  std::bind(&isr, sensorInfo.echoPin, &sensorInfo.start, &sensorInfo.end), CHANGE);
+  TOFManager::startupManager(sensor_queue);
 }
 
-void HCSR04SensorManager::detachInterrupts() {
-  for (auto & sensor : m_sensors) {
-    detachInterrupt(sensor.echoPin);
-  }
-}
-
-void HCSR04SensorManager::attachInterrupts() {
-  for (size_t idx = 0; idx < NUMBER_OF_TOF_SENSORS; ++idx) {
-    attachSensorInterrupt(idx);
-  }
-}
+void DistanceSensorManager::reset(uint32_t startMillisTicks) {
+  log_d("Reset");
 
-void HCSR04SensorManager::reset(uint32_t startMillisTicks) {
   for (auto & sensor : m_sensors) {
     sensor.minDistance = MAX_SENSOR_VALUE;
     memset(&(sensor.echoDurationMicroseconds), 0, sizeof(sensor.echoDurationMicroseconds));
     sensor.numberOfTriggers = 0;
+    delete sensor.median;
+    sensor.median = new Median<uint16_t>(5, MAX_SENSOR_VALUE);
+    sensor.rawDistance = MAX_SENSOR_VALUE;
   }
+
+  memset(&(captureOffsetMS), 0, sizeof(captureOffsetMS));
+
   lastReadingCount = 0;
-  lastSensor = 1 - primarySensor;
-  memset(&(startOffsetMilliseconds), 0, sizeof(startOffsetMilliseconds));
-  startReadingMilliseconds = startMillisTicks;
+  sensorStartTimestampMS = startMillisTicks;
 }
 
-void HCSR04SensorManager::setOffsets(std::vector<uint16_t> offsets) {
+void DistanceSensorManager::setOffsets(std::vector<uint16_t> offsets) {
   for (size_t idx = 0; idx < NUMBER_OF_TOF_SENSORS; ++idx) {
     if (idx < offsets.size()) {
-      m_sensors[idx].offset = offsets[idx];
+      getSensor(idx)->offset = offsets[idx];
     } else {
-      m_sensors[idx].offset = 0;
+      getSensor(idx)->offset = 0;
     }
   }
 }
 
-/* The primary sensor defines the measurement interval, we trigger a measurement if this
- * sensor is ready.
- */
-void HCSR04SensorManager::setPrimarySensor(uint8_t idx) {
-  primarySensor = idx;
-}
-
 /* Polls for new readings, if sensors are not ready, the
  * method returns false.
  */
-bool HCSR04SensorManager::pollDistancesAlternating() {
+bool DistanceSensorManager::pollDistances() {
   bool newMeasurements = false;
-  if (lastSensor == primarySensor && isReadyForStart(1 - primarySensor)) {
-    setSensorTriggersToLow();
-    lastSensor = 1 - primarySensor;
-    sendTriggerToSensor(1 - primarySensor);
-  } else if (isReadyForStart(primarySensor)) {
-    newMeasurements = collectSensorResults();
-    setSensorTriggersToLow();
-    lastSensor = primarySensor;
-    sendTriggerToSensor(primarySensor);
+  LLMessage message = {0};
+
+  while(xQueueReceive(sensor_queue, &message, 5) == pdPASS) {
+    // // woohoo! we received a new measurement
+    newMeasurements = true;
+    processSensorMessage(&message);
   }
+
   return newMeasurements;
 }
 
-/* Polls for new readings, if sensors are not ready, the
- * method returns false. If sensors are ready and readings
- * are available, data is read and updated. If the primary
- * sensor is ready for new measurement, a fresh measurement
- * is triggered. Method returns true if new data (no timeout
- * measurement) was collected.
- */
-bool HCSR04SensorManager::pollDistancesParallel() {
-  bool newMeasurements = false;
-  if (isReadyForStart(primarySensor)) {
-    setSensorTriggersToLow();
-    newMeasurements = collectSensorResults();
-    const bool secondSensorIsReady = isReadyForStart(1 - primarySensor);
-    sendTriggerToSensor(primarySensor);
-    if (secondSensorIsReady) {
-      sendTriggerToSensor(1 - primarySensor);
-    }
+uint16_t DistanceSensorManager::computeDistance(uint32_t travel_time) {
+  uint16_t distance = MAX_SENSOR_VALUE;
+  if (travel_time > MIN_DURATION_MICRO_SEC || travel_time <= MAX_DURATION_MICRO_SEC) {
+    distance = static_cast<uint16_t>(travel_time / MICRO_SEC_TO_CM_DIVIDER);
   }
-  return newMeasurements;
+  return distance;
 }
 
-uint16_t HCSR04SensorManager::getCurrentMeasureIndex() {
-  return lastReadingCount - 1;
+void DistanceSensorManager::proposeMinDistance(DistanceSensor* const sensor, uint16_t distance) {
+ if ((distance > 0) && (distance < sensor->minDistance)) {
+    sensor->minDistance = distance;
+  }
 }
 
-/* Send echo trigger to the selected sensor and prepare measurement data
- * structures.
- */
-void HCSR04SensorManager::sendTriggerToSensor(uint8_t sensorId) {
-  HCSR04SensorInfo * const sensor = &(m_sensors[sensorId]);
-  updateStatistics(sensor);
-  sensor->end = MEASUREMENT_IN_PROGRESS; // will be updated with LOW signal
-  sensor->numberOfTriggers++;
-  sensor->measurementRead = false;
-  const uint32_t now = micros();
-  sensor->trigger = now; // will be updated with HIGH signal
-  sensor->start = now;
-  digitalWrite(sensor->triggerPin, HIGH);
-  // 10us are specified but some sensors are more stable with 20us according
-  // to internet reports
-  delayMicroseconds(20);
-  digitalWrite(sensor->triggerPin, LOW);
-}
+// FIXME
+/* Returns true if there was a no timeout reading. */
+bool DistanceSensorManager::processSensorMessage(LLMessage* message) {
+  log_d("Processing sensor message from sensor = %i", message->sensor_index);
+  DistanceSensor* const sensor = &m_sensors[message->sensor_index];
+  bool reading_valid = false;
 
-/* Checks if the given sensor is ready for a new measurement cycle.
- */
-boolean HCSR04SensorManager::isReadyForStart(uint8_t sensorId) {
-  HCSR04SensorInfo * const sensor = &m_sensors[sensorId];
-  boolean ready = false;
-  const uint32_t now = micros();
-  const uint32_t start = sensor->start;
-  const uint32_t end = sensor->end;
-  if (end != MEASUREMENT_IN_PROGRESS) { // no measurement in flight or just finished
-    const uint32_t startOther = m_sensors[1 - sensorId].start;
-    if (   (microsBetween(now, end) > SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC)
-        && (microsBetween(now, start) > SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC)
-        && (microsBetween(now, startOther) > SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC)) {
-      ready = true;
-    }
-    if (digitalRead(sensor->echoPin) != LOW) {
-      log_e("Measurement done, but echo pin is high for %s sensor", sensor->sensorLocation);
-      sensor->numberOfLowAfterMeasurement++;
-    }
-  } else if (microsBetween(now, start) > MAX_TIMEOUT_MICRO_SEC) {
-    sensor->numberOfToLongMeasurement++;
-    // signal or interrupt was lost altogether this is an error,
-    // should we raise it?? Now pretend the sensor is ready, hope it helps to give it a trigger.
-    ready = true;
-    // do not log to much there might be devices out there with one sensor only
-    log_d("Timeout trigger for %s duration %u us - echo pin state: %d trigger: %u "
-        "start: %u end: %u now: %u",
-      sensor->sensorLocation, now - start, digitalRead(sensor->echoPin), sensor->trigger,
-        start, end, now);
+  uint32_t duration = microsBetween(message->start, message->end);
+
+  sensor->echoDurationMicroseconds[lastReadingCount] = duration;
+
+  if (duration > MIN_DURATION_MICRO_SEC && duration < MAX_DURATION_MICRO_SEC) {
+    // these values should only be set if we have correct data
+    uint16_t rawDistance = computeDistance(duration);
+    sensor->median->addValue(rawDistance);
+
+    sensor->rawDistance = rawDistance;
+
+    uint16_t offsetDistance = computeOffsetDistance(medianMeasure(sensor, rawDistance), sensor->offset);
+    sensor->distance = offsetDistance;
+    proposeMinDistance(sensor, offsetDistance);
+    reading_valid = true;
+  } else {
+    sensor->rawDistance = MAX_SENSOR_VALUE;
+    sensor->distance = MAX_SENSOR_VALUE;
   }
-  return ready;
-}
 
-bool HCSR04SensorManager::collectSensorResults() {
-  bool validReading = false;
-  for (size_t idx = 0; idx < NUMBER_OF_TOF_SENSORS; ++idx) {
-    if (collectSensorResult(idx)) {
-      validReading = true;
+  captureOffsetMS[lastReadingCount] = millisSince(sensorStartTimestampMS);
+
+  if(message->sensor_index == 1) {
+    if (lastReadingCount < MAX_NUMBER_MEASUREMENTS_PER_INTERVAL - 1) {
+      lastReadingCount++;
     }
   }
-  if (validReading) {
-    registerReadings();
-  }
-  return validReading;
+
+  return reading_valid;
 }
 
-void HCSR04SensorManager::registerReadings() {
-  startOffsetMilliseconds[lastReadingCount] = millisSince(startReadingMilliseconds);
-  if (lastReadingCount < MAX_NUMBER_MEASUREMENTS_PER_INTERVAL - 1) {
-    lastReadingCount++;
-  }
+uint16_t DistanceSensorManager::getSensorRawDistance(uint8_t sensorIndex) {
+  return getSensor(sensorIndex)->rawDistance;
 }
 
-/* Returns true if there was a no timeout reading. */
-bool HCSR04SensorManager::collectSensorResult(uint8_t sensorId) {
-  HCSR04SensorInfo* const sensor = &m_sensors[sensorId];
-  if (sensor->measurementRead) {
-    return false; // already read
-  }
-  bool validReading = false;
-  const uint32_t end = sensor->end;
-  const uint32_t start = getFixedStart(sensorId, sensor);
-  uint32_t duration;
-  if (end == MEASUREMENT_IN_PROGRESS) {
-    duration = microsSince(start);
-    if (duration < MAX_DURATION_MICRO_SEC) {
-      return false;  // still measuring
-    }
-    // measurement is still in flight! But the time we want to wait is up (> MAX_DURATION_MICRO_SEC)
-    sensor->echoDurationMicroseconds[lastReadingCount] = -1;
-  } else {
-    duration = microsBetween(start, end);
-    sensor->echoDurationMicroseconds[lastReadingCount] = duration;
-  }
-  uint16_t dist;
-  if (duration < MIN_DURATION_MICRO_SEC || duration >= MAX_DURATION_MICRO_SEC) {
-    dist = MAX_SENSOR_VALUE;
-  } else {
-    validReading = true;
-    dist = static_cast<uint16_t>(duration / MICRO_SEC_TO_CM_DIVIDER);
-  }
-  sensor->rawDistance = dist;
-  sensor->median->addValue(dist);
-  sensorValues[sensorId] =
-    sensor->distance = correctSensorOffset(medianMeasure(sensor, dist), sensor->offset);
+DistanceSensor* DistanceSensorManager::getSensor(uint8_t sensorIndex) {
+  return &m_sensors[sensorIndex];
+}
 
-  log_v("Raw sensor[%d] distance read %03u / %03u (%03u, %03u, %03u) -> *%03ucm*, duration: %zu us - echo pin state: %d",
-    sensorId, sensor->rawDistance, dist, sensor->distances[0], sensor->distances[1],
-    sensor->distances[2], sensorValues[sensorId], duration, digitalRead(sensor->echoPin));
+boolean DistanceSensorManager::hasReadings(uint8_t sensorIndex) {
+  return lastReadingCount > 0;
+}
 
-  if (sensor->distance > 0 && sensor->distance < sensor->minDistance) {
-    sensor->minDistance = sensor->distance;
+void DistanceSensorManager::copyData(DataSet* set) {
+  log_d("Let's copy some data.");
+  for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i++){
+    set->sensorValues.push_back(m_sensors[i].minDistance);
+  }
+
+  set->measurements = lastReadingCount;
+  memcpy(&(set->readDurationsRightInMicroseconds),
+         &(m_sensors[0].echoDurationMicroseconds), set->measurements * sizeof(int32_t));
+  memcpy(&(set->readDurationsLeftInMicroseconds),
+         &(m_sensors[1].echoDurationMicroseconds), set->measurements * sizeof(int32_t));
+  memcpy(&(set->startOffsetMilliseconds),
+         &(captureOffsetMS), set->measurements * sizeof(uint16_t));
+}
+
+uint16_t DistanceSensorManager::getLastValidRawDistance(uint8_t sensorIndex) {
+  // if there was no measurement of this sensor for the current index, it is the
+  // one before. This happens with fast confirmations.
+  uint16_t val = 0;
+  for (int i = lastReadingCount - 1; i >= 0; i--) {
+    uint16_t maybeVal = getSensor(sensorIndex)->echoDurationMicroseconds[i];
+    if (maybeVal != 0) {
+      val = maybeVal;
+      break;
+    }
   }
-  sensor->measurementRead = true;
-  return validReading;
+  return val;
 }
 
-uint16_t HCSR04SensorManager::getRawMedianDistance(uint8_t sensorId) {
+uint16_t DistanceSensorManager::getMedianRawDistance(uint8_t sensorId) {
  return m_sensors[sensorId].median->median();
 }
 
-uint32_t HCSR04SensorManager::getMaxDurationUs(uint8_t sensorId) {
+uint32_t DistanceSensorManager::getMaxDurationUs(uint8_t sensorId) {
   return m_sensors[sensorId].maxDurationUs;
 }
 
-uint32_t HCSR04SensorManager::getMinDurationUs(uint8_t sensorId) {
+uint32_t DistanceSensorManager::getMinDurationUs(uint8_t sensorId) {
   return m_sensors[sensorId].minDurationUs;
 }
 
-uint32_t HCSR04SensorManager::getLastDelayTillStartUs(uint8_t sensorId) {
+uint32_t DistanceSensorManager::getLastDelayTillStartUs(uint8_t sensorId) {
   return m_sensors[sensorId].lastDelayTillStartUs;
 }
 
-uint32_t HCSR04SensorManager::getNoSignalReadings(const uint8_t sensorId) {
+uint32_t DistanceSensorManager::getNoSignalReadings(const uint8_t sensorId) {
   return m_sensors[sensorId].numberOfNoSignals;
 }
 
-uint32_t HCSR04SensorManager::getNumberOfLowAfterMeasurement(const uint8_t sensorId) {
+uint32_t DistanceSensorManager::getNumberOfLowAfterMeasurement(const uint8_t sensorId) {
   return m_sensors[sensorId].numberOfLowAfterMeasurement;
 }
 
-uint32_t HCSR04SensorManager::getNumberOfToLongMeasurement(const uint8_t sensorId) {
+uint32_t DistanceSensorManager::getNumberOfToLongMeasurement(const uint8_t sensorId) {
   return m_sensors[sensorId].numberOfToLongMeasurement;
 }
 
-uint32_t HCSR04SensorManager::getNumberOfInterruptAdjustments(const uint8_t sensorId) {
+uint32_t DistanceSensorManager::getNumberOfInterruptAdjustments(const uint8_t sensorId) {
   return m_sensors[sensorId].numberOfInterruptAdjustments;
 }
 
@@ -396,9 +236,11 @@ uint32_t HCSR04SensorManager::getNumberOfInterruptAdjustments(const uint8_t sens
  * sensor this adds 300 microseconds or 5 centimeters to the measured result.
  * After research, is a bug in the ESP!? See https://esp32.com/viewtopic.php?t=10124
  */
-uint32_t HCSR04SensorManager::getFixedStart(
+// FIXME
+/* uint32_t HCSR04SensorManager::getFixedStart(
+  uint32_t start = 0;
   size_t idx, HCSR04SensorInfo * const sensor) {
-  uint32_t start = sensor->start;
+   uint32_t start = sensor->start;
   // the error appears if both sensors trigger the interrupt at the exact same
   // time, if this happens, trigger time == start time
   if (start == sensor->trigger && sensor->end != MEASUREMENT_IN_PROGRESS) {
@@ -412,8 +254,9 @@ uint32_t HCSR04SensorManager::getFixedStart(
   }
   return start;
 }
+ */
 
-uint16_t HCSR04SensorManager::correctSensorOffset(uint16_t dist, uint16_t offset) {
+uint16_t DistanceSensorManager::computeOffsetDistance(uint16_t dist, uint16_t offset) {
   uint16_t  result;
   if (dist == MAX_SENSOR_VALUE) {
     result = MAX_SENSOR_VALUE;
@@ -425,28 +268,25 @@ uint16_t HCSR04SensorManager::correctSensorOffset(uint16_t dist, uint16_t offset
   return result;
 }
 
-void HCSR04SensorManager::setSensorTriggersToLow() {
-  for (auto & m_sensor : m_sensors) {
-    digitalWrite(m_sensor.triggerPin, LOW);
-  }
-}
+void DistanceSensorManager::updateStatistics(LLMessage* message) {
+#if 1
+  DistanceSensor* sensor = getSensor(message->sensor_index);
 
-void HCSR04SensorManager::updateStatistics(HCSR04SensorInfo * const sensor) {
-  if (sensor->end != MEASUREMENT_IN_PROGRESS) {
-    const uint32_t startDelay = sensor->start - sensor->trigger;
-    if (startDelay != 0) {
-      const uint32_t duration = sensor->end - sensor->start;
-      if (duration > sensor->maxDurationUs) {
-        sensor->maxDurationUs = duration;
-      }
-      if (duration < sensor->minDurationUs && duration > 1) {
-        sensor->minDurationUs = duration;
-      }
-      sensor->lastDelayTillStartUs = startDelay;
+  auto start_delay = message->start - message->trigger;
+  if (start_delay > 0) {
+    sensor->lastDelayTillStartUs = start_delay;
+
+    auto duration = message->end - message->start;
+    if (duration > sensor->maxDurationUs) {
+      sensor->maxDurationUs = duration;
+    }
+    if (duration < sensor->minDurationUs && duration > 1) {
+      sensor->minDurationUs = duration;
     }
   } else {
     sensor->numberOfNoSignals++;
   }
+#endif
 }
 
 /* Determines the microseconds between the 2 time counters given.
@@ -456,7 +296,7 @@ void HCSR04SensorManager::updateStatistics(HCSR04SensorInfo * const sensor) {
  * We should not use 64bit variables for "start" and "end" because access
  * to 64bit vars is not atomic for our 32bit cpu.
  */
-uint32_t HCSR04SensorManager::microsBetween(uint32_t a, uint32_t b) {
+uint32_t DistanceSensorManager::microsBetween(uint32_t a, uint32_t b) {
   uint32_t result = a - b;
   if (result & 0x80000000) {
     result = -result;
@@ -469,11 +309,11 @@ uint32_t HCSR04SensorManager::microsBetween(uint32_t a, uint32_t b) {
  * so we take care for the overflow. Times we measure are way below the
  * possible maximum.
  */
-uint32_t HCSR04SensorManager::microsSince(uint32_t a) {
+uint32_t DistanceSensorManager::microsSince(uint32_t a) {
   return microsBetween(micros(), a);
 }
 
-uint16_t HCSR04SensorManager::millisSince(uint16_t milliseconds) {
+uint16_t DistanceSensorManager::millisSince(uint16_t milliseconds) {
   uint16_t result = ((uint16_t) millis()) - milliseconds;
   if (result & 0x8000) {
     result = -result;
@@ -481,7 +321,7 @@ uint16_t HCSR04SensorManager::millisSince(uint16_t milliseconds) {
   return result;
 }
 
-uint16_t HCSR04SensorManager::medianMeasure(HCSR04SensorInfo *const sensor, uint16_t value) {
+uint16_t DistanceSensorManager::medianMeasure(DistanceSensor *const sensor, uint16_t value) {
   sensor->distances[sensor->nextMedianDistance] = value;
   sensor->nextMedianDistance++;
 
@@ -498,7 +338,7 @@ uint16_t HCSR04SensorManager::medianMeasure(HCSR04SensorInfo *const sensor, uint
   return median(sensor->distances[0], sensor->distances[1], sensor->distances[2]);
 }
 
-uint16_t HCSR04SensorManager::median(uint16_t a, uint16_t b, uint16_t c) {
+uint16_t DistanceSensorManager::median(uint16_t a, uint16_t b, uint16_t c) {
   if (a < b) {
     if (a >= c) {
       return a;
diff --git a/src/sensor.h b/src/sensor.h
index 443c684b..93a11a0b 100644
--- a/src/sensor.h
+++ b/src/sensor.h
@@ -24,12 +24,15 @@
 #ifndef OBS_SENSOR_H
 #define OBS_SENSOR_H
 
-#include <vector>
 #include <Arduino.h>
+#include <vector>
 
-#include "globals.h"
 #include "utils/median.h"
 
+#include "tofmanager.h"
+
+struct DataSet;
+
 /* About the speed of sound:
    See also http://www.sengpielaudio.com/Rechner-schallgeschw.htm (german)
     - speed of sound depends on ambient temperature
@@ -58,28 +61,39 @@ extern const uint16_t MAX_SENSOR_VALUE;
 
 const uint8_t NUMBER_OF_TOF_SENSORS = 2;
 
-struct HCSR04SensorInfo {
-  uint8_t triggerPin = 15;
-  uint8_t echoPin = 4;
+struct DistanceSensor {
+  // offset from the sensor to the end of the handlebar (cm)
   uint16_t offset = 0;
+  // the distance from the sensor to some object (cm)
   uint16_t rawDistance = 0;
+  // median of the offset distance (cm)
   uint16_t distances[MEDIAN_DISTANCE_MEASURES] = { MAX_SENSOR_VALUE, MAX_SENSOR_VALUE, MAX_SENSOR_VALUE };
+  // index into the previous member
   uint16_t nextMedianDistance = 0;
+  // minimum offset distance the sensor noticed between resets (cm)
   uint16_t minDistance = MAX_SENSOR_VALUE;
+  // current offset distance of the sensor (cm)
   uint16_t distance = MAX_SENSOR_VALUE;
-  char* sensorLocation;
-  // timestamp when the trigger signal was sent in us micros()
-  uint32_t trigger = 0;
-  volatile uint32_t start = 0;
-  /* if end == 0 - a measurement is in progress */
-  volatile uint32_t end = 1;
 
+  // UI string that denotes the sensor position
+  char const* sensorLocation;
+
+  // an array with all of the time-of-flight measurements since the last reset
   int32_t echoDurationMicroseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1];
+
+  // a data structure for computing the median offset distance since the last reset (cm)
   Median<uint16_t>*median = nullptr;
+
   // statistics
+
+  // maximum time-of-flight the sensor measured since the last reset (microseconds)
   uint32_t maxDurationUs = 0;
+  // minimum time-of-flight the sensor measured since the last reset (microseconds)
   uint32_t minDurationUs = UINT32_MAX;
+  // internal delay to start metric
   uint32_t lastDelayTillStartUs = 0;
+
+  // FIXME: internal statistics
   // counts how often no echo and also no timeout signal was received
   // should only happen with defect or missing sensors
   uint32_t numberOfNoSignals = 0;
@@ -87,25 +101,21 @@ struct HCSR04SensorInfo {
   uint32_t numberOfToLongMeasurement = 0;
   uint32_t numberOfInterruptAdjustments = 0;
   uint16_t numberOfTriggers = 0;
-  bool measurementRead;
 };
 
-class HCSR04SensorManager {
+class DistanceSensorManager {
   public:
-    HCSR04SensorManager() {}
-    virtual ~HCSR04SensorManager() {}
+    DistanceSensorManager() {}
+    virtual ~DistanceSensorManager() {}
     void reset(uint32_t startMillisTicks);
-    void registerSensor(const HCSR04SensorInfo &, uint8_t idx);
     void setOffsets(std::vector<uint16_t>);
-    void setPrimarySensor(uint8_t idx);
-    void detachInterrupts();
-    void attachInterrupts();
+    void initSensors();
+
     /* Returns the current raw median distance in cm for the
      * given sensor.
      */
-    uint16_t getRawMedianDistance(uint8_t sensorId);
+    uint16_t getMedianRawDistance(uint8_t sensorId);
     /* Index for CSV. */
-    uint16_t getCurrentMeasureIndex();
     uint32_t getMaxDurationUs(uint8_t sensorId);
     uint32_t getMinDurationUs(uint8_t sensorId);
     uint32_t getLastDelayTillStartUs(uint8_t sensorId);
@@ -114,34 +124,41 @@ class HCSR04SensorManager {
     uint32_t getNumberOfToLongMeasurement(const uint8_t sensorId);
     uint32_t getNumberOfInterruptAdjustments(const uint8_t sensorId);
 
-    HCSR04SensorInfo m_sensors[NUMBER_OF_TOF_SENSORS];
-    uint16_t sensorValues[NUMBER_OF_TOF_SENSORS];
-    uint16_t lastReadingCount = 0;
-    uint16_t startOffsetMilliseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1];
-    bool pollDistancesParallel();
-    bool pollDistancesAlternating();
+    DistanceSensor* getSensor(uint8_t sensorIndex);
+    uint16_t getSensorRawDistance(uint8_t sensorIndex);
+    uint16_t getLastValidRawDistance(uint8_t sensorIndex);
+    boolean hasReadings(uint8_t sensorIndex);
+    void copyData(DataSet* set);
+
+    bool pollDistances();
 
   protected:
+    uint16_t captureOffsetMS[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1];
+    DistanceSensor m_sensors[NUMBER_OF_TOF_SENSORS];
+    void updateStatistics(LLMessage* message);
+    uint16_t lastReadingCount = 0;
+
+    /* offer a new value for the minimum distance */
+    void proposeMinDistance(DistanceSensor* const sensor, uint16_t distance);
+
+    /* compute the distance to an object considering `travel_time` */
+    uint16_t computeDistance(uint32_t travel_time);
 
   private:
-    void sendTriggerToSensor(uint8_t sensorId);
-    bool collectSensorResult(uint8_t sensorId);
-    void setSensorTriggersToLow();
-    bool collectSensorResults();
-    void attachSensorInterrupt(uint8_t idx);
-    uint32_t getFixedStart(size_t idx, HCSR04SensorInfo * const sensor);
-    boolean isReadyForStart(uint8_t sensorId);
-    void registerReadings();
-    static uint16_t medianMeasure(HCSR04SensorInfo* const sensor, uint16_t value);
+    static uint16_t medianMeasure(DistanceSensor* const sensor, uint16_t value);
     static uint16_t median(uint16_t a, uint16_t b, uint16_t c);
-    static uint16_t correctSensorOffset(uint16_t dist, uint16_t offset);
+    static uint16_t computeOffsetDistance(uint16_t dist, uint16_t offset);
     static uint32_t microsBetween(uint32_t a, uint32_t b);
     static uint32_t microsSince(uint32_t a);
     static uint16_t millisSince(uint16_t milliseconds);
-    static void updateStatistics(HCSR04SensorInfo * const sensor);
-    uint32_t startReadingMilliseconds = 0;
-    uint8_t primarySensor = 1;
-    uint8_t lastSensor;
+
+    bool processSensorMessage(LLMessage* message);
+    uint32_t getFixedStart(size_t idx, DistanceSensor * const sensor);
+    uint32_t sensorStartTimestampMS = 0;
+
+    QueueHandle_t sensor_queue;
+    uint8_t* queue_buffer;
+    StaticQueue_t *static_queue;
 };
 
 #endif
diff --git a/src/tofmanager.cpp b/src/tofmanager.cpp
new file mode 100644
index 00000000..4c609324
--- /dev/null
+++ b/src/tofmanager.cpp
@@ -0,0 +1,320 @@
+#include "tofmanager.h"
+#include "sensor.h"
+
+#include <unistd.h>
+
+/* Value of HCSR04SensorInfo::end during an ongoing measurement. */
+#define MEASUREMENT_IN_PROGRESS 0
+
+/* To avoid that we receive a echo from a former measurement, we do not
+ * start a new measurement within the given time from the start of the
+ * former measurement of the opposite sensor.
+ * High values can lead to the situation that we only poll the
+ * primary sensor for a while!?
+ */
+#define SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC (30 * 1000)
+
+/* The last end (echo goes to low) of a measurement must be this far
+ * away before a new measurement is started.
+ */
+#define SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC (10 * 1000)
+
+/* The last start of a new measurement must be as long ago as given here
+    away, until we start a new measurement.
+   - With 35ms I could get stable readings down to 19cm (new sensor)
+   - With 30ms I could get stable readings down to 25/35cm only (new sensor)
+   - It looked fine with the old sensor for all values
+ */
+#define SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC (35 * 1000)
+
+/* This time is maximum echo pin high time if the sensor gets no response
+ * HC-SR04: observed 71ms, docu 60ms
+ * JSN-SR04T: observed 58ms
+ */
+#define MAX_TIMEOUT_MICRO_SEC 75000
+
+/* The core to run the RT component on */
+#define RT_CORE 0
+
+/* Delay to call when we're waiting for work */
+#define TICK_DELAY 1
+
+/* Enable init code for running alone */
+#define ALONE_ON_CORE 1
+
+/* default interrupt flags */
+#define OBS_INTR_FLAG 0
+
+/* Some calculations:
+ *
+ * Assumption:
+ *  Small car length: 4m (average 2017 4,4m, really smallest is 2.3m )
+ *  Speed difference: 100km/h = 27.8m/s
+ *  Max interesting distance: MAX_DISTANCE_MEASURED_CM = 250cm
+ *
+ * Measured:
+ *  MAX_TIMEOUT_MICRO_SEC == ~ 75_000 micro seconds, if the sensor gets no echo
+ *
+ * Time of the overtake process:
+ *  4m / 27.8m/s = 0.143 sec
+ *
+ * We need to have time for 3 measures to get the car
+ *
+ * 0.148 sec / 3 measures = 48 milli seconds pre measure = 48_000 micro_sec
+ *
+ * -> If we wait till MAX_TIMEOUT_MICRO_SEC (75ms) because the right
+ *    sensor has no measure we are to slow. (3x75 = 225ms > 143ms)
+ *
+ *    If we only waif tor the primary (left) sensor we are good again,
+ *    and so trigger one sensor while the other is still in "timeout-state"
+ *    we save some time. Assuming 1st measure is just before the car appears
+ *    besides the sensor:
+ *
+ *    75ms open sensor + 2x SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC (35ms) = 145ms which is
+ *    close to the needed 148ms
+ */
+
+/* Send echo trigger to the sensor `sensorId` and prepare measurement data
+ * structures.
+ */
+void TOFManager::startMeasurement(uint8_t sensorId) {
+  LLSensor* sensor = sensors(sensorId);
+
+  sensor->started_measurements += 1;
+
+  sensor->end = MEASUREMENT_IN_PROGRESS; // will be updated with LOW signal
+  unsigned long now = esp_timer_get_time();
+
+  sensor->trigger = now;
+  sensor->start = 0; // will be updated with HIGH signal
+
+  gpio_set_level(sensor->triggerPin, 1);
+  // 10us are specified but some sensors are more stable with 20us according
+  // to internet reports
+  usleep(20);
+  gpio_set_level(sensor->triggerPin, 0);
+}
+
+/* Check if the sensor `sensorId` is ready for a new measurement cycle.
+ */
+boolean TOFManager::isSensorReady(uint8_t sensorId) {
+  LLSensor* sensor = sensors(sensorId);
+  boolean ready = false;
+  auto now = esp_timer_get_time();
+  auto start = sensor->start.load();
+  auto end = sensor->end.load();
+
+  if (end != MEASUREMENT_IN_PROGRESS) { // no measurement in flight or just finished
+    auto startOther = sensors(1 - sensorId)->start.load();
+    if ((now - end) > SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC
+        && (now - start) > SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC
+        && (now - startOther) > SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC) {
+      ready = true;
+      sensors(sensorId)->successful_measurements += 1;
+    }
+  } else if (now - sensor->trigger > MAX_TIMEOUT_MICRO_SEC) {
+    // signal or interrupt was lost altogether this is an error,
+    // should we raise a  it?? Now pretend the sensor is ready, hope it helps to give it a trigger.
+    ready = true;
+    sensors(sensorId)->lost_signals += 1;
+  }
+  return ready;
+}
+
+/* Check if the measurement on the given sensor `sensor_idx` is complete. */
+boolean TOFManager::isMeasurementComplete(uint8_t sensor_idx) {
+  if (sensors(sensor_idx)->end != MEASUREMENT_IN_PROGRESS) {
+    return true;
+  }
+  uint32_t now = esp_timer_get_time();
+  if (sensors(sensor_idx)->trigger + MAX_TIMEOUT_MICRO_SEC < now) {
+    return true;
+  }
+
+  return false;
+}
+
+/* Initialize the sensor manager and start it up. Use this to start the sensor.
+ * This function will start a new FreeRTOS task. */
+void TOFManager::startupManager(QueueHandle_t queue) {
+  esp_log_level_set("tofmanager", ESP_LOG_INFO);
+  ESP_LOGI("tofmanager","Currently on core %i", xPortGetCoreID());
+  ESP_LOGI("tofmanager","Switching to core %i", RT_CORE);
+
+  xTaskCreatePinnedToCore(
+    TOFManager::sensorTaskFunction,          /* Task function. */
+    "TOFManager",        /* String with name of task. */
+    16 * 1024,           /* Stack size in bytes. */
+    queue,               /* Parameter passed as input of the task */
+    1,                   /* Priority of the task. */
+    NULL,                /* Task handle. */
+    RT_CORE              /* the core to run on */
+  );
+}
+
+/* Main function that will be started by startupManager().
+ * This function will never return. This function is supposed to run in its own
+ * FreeRTOS task. */
+void TOFManager::sensorTaskFunction(void *parameter) {
+  esp_log_level_set("tofmanager", ESP_LOG_INFO);
+  isr_log_i("tofmanager", "Hello from core %i!", xPortGetCoreID());
+
+  LLSensor* sensors = new LLSensor[2];
+
+  TOFManager* sensorManager = new TOFManager((QueueHandle_t)parameter, sensors);
+
+  if (sensors == 0 || sensorManager == 0) {
+    ESP_LOGE("tofmanager","Error, apparently we've run out of memory (sensors=%x, sensorManager=%x).",
+      sensors, sensorManager);
+  }
+
+  sensorManager->init();
+  sensorManager->loop();
+}
+
+/* Initialize the TOFManager: setup sensor pins and interrupts. */
+void TOFManager::init() {
+#if ALONE_ON_CORE
+  esp_timer_init();
+  gpio_install_isr_service(OBS_INTR_FLAG);
+#endif
+
+  LLSensor* sensorManaged1 = sensors(0);
+  sensorManaged1->triggerPin = GPIO_NUM_15;
+  sensorManaged1->echoPin = GPIO_NUM_4;
+  setupSensor(sensorManaged1, 0);
+
+  LLSensor* sensorManaged2 = sensors(1);
+  sensorManaged2->triggerPin = GPIO_NUM_25;
+  sensorManaged2->echoPin = GPIO_NUM_26;
+  setupSensor(sensorManaged2, 1);
+
+  ESP_LOGI("tofmanager", "TOFManager init complete");
+}
+
+/* Looking at a single sensor `sensor_idx`, take its data and
+ * send it off to the high level sensor component. */
+void TOFManager::sendData(uint8_t sensor_idx) {
+  LLSensor* sensor = sensors(sensor_idx);
+
+  LLMessage message = {0};
+
+  message.sensor_index = sensor_idx;
+  message.trigger = sensor->trigger;
+  message.start = sensor->start;
+  message.end = sensor->end;
+  xQueueSendToBack(sensor_queue, &message, portMAX_DELAY);
+}
+
+/* The main loop of the TOFManager. This function will never return.
+ * It sequences all measurements and sends their data to the high leve component
+ * of the sensor code. */
+void TOFManager::loop() {
+  ESP_LOGI("tofmanager","Entering TOFManager::loop");
+  currentSensor = primarySensor;
+  startMeasurement(primarySensor);
+
+  float startLoop = esp_timer_get_time();
+
+  // this toggle switches once approx every 8 seconds
+  uint8_t logToggle = 0;
+
+  while ( true ) {
+    uint8_t newToggle = (esp_timer_get_time() >> 23) & 0x1;
+    if (newToggle != logToggle) {
+      float spent = ((float)esp_timer_get_time() - startLoop) / 1000000.0;
+      ESP_LOGI("tofmanager", "stats: sensor1->irq_cnt=%u, sensor2->irq_cnt=%u",
+        sensors(0)->interrupt_count, sensors(1)->interrupt_count);
+      ESP_LOGI("tofmanager", "stats: success=(%u, %u), started=(%u,%u)",
+        sensors(0)->successful_measurements,
+        sensors(1)->successful_measurements,
+        sensors(0)->started_measurements,
+        sensors(1)->started_measurements);
+      ESP_LOGI("tofmanager", "stats: (s0) trigger-start-end = %x-%x-%x",
+        sensors(0)->trigger.load(), sensors(0)->start.load(), sensors(0)->end.load());
+      ESP_LOGI("tofmanager", "stats: (s1) trigger-start-end = %x-%x-%x",
+        sensors(1)->trigger.load(), sensors(1)->start.load(), sensors(1)->end.load());
+      if (spent > 0) {
+        ESP_LOGI("tofmanager", "statlog: (%f,%f) started/s",
+          (float)sensors(0)->started_measurements / spent,
+          (float)sensors(0)->started_measurements / spent);
+      }
+      logToggle = newToggle;
+    }
+
+    // wait for sensor
+    if (!isMeasurementComplete(currentSensor)) {
+      vTaskDelay(1);
+      //taskYIELD();
+    } else {
+      // switch to next sensor & send out data
+      if (currentSensor == primarySensor && isSensorReady(1 - primarySensor)) {
+        sendData(currentSensor);
+        disableMeasurement(currentSensor);
+        currentSensor = 1 - primarySensor;
+        startMeasurement(currentSensor);
+      } else if (isSensorReady(primarySensor)) {
+        sendData(currentSensor);
+        disableMeasurement(currentSensor);
+        currentSensor = primarySensor;
+        startMeasurement(primarySensor);
+      }
+    }
+  }
+}
+
+/* Sensor interrupt function. Use the `isrParam` to pass in a LLSensor*
+ * Fetch the current phase of that sensor and set it's values. */
+static void IRAM_ATTR sensorInterrupt(void *isrParam) {
+  // since the measurement of start and stop use the same interrupt
+  // mechanism we should see a similar delay.
+  LLSensor* sensor = (LLSensor*) isrParam;
+
+  sensor->interrupt_count += 1;
+  if (sensor->end == MEASUREMENT_IN_PROGRESS) {
+    if (sensor->start == 0) {
+      sensor->start = esp_timer_get_time();
+    } else {
+      sensor->end = esp_timer_get_time();
+    }
+  }
+}
+
+/* Set up a single `sensor` including its pins and interrupts. */
+void TOFManager::setupSensor(LLSensor* sensor, uint8_t idx) {
+  gpio_config_t io_conf_trigger = {};
+  io_conf_trigger.intr_type = GPIO_INTR_DISABLE;
+  io_conf_trigger.mode = GPIO_MODE_OUTPUT;
+  io_conf_trigger.pin_bit_mask = 1ULL << sensor->triggerPin;
+  io_conf_trigger.pull_down_en = GPIO_PULLDOWN_DISABLE;
+  io_conf_trigger.pull_up_en = GPIO_PULLUP_DISABLE;
+  gpio_config(&io_conf_trigger);
+
+  gpio_config_t io_conf_echo = {};
+  io_conf_echo.intr_type = GPIO_INTR_ANYEDGE;
+  io_conf_echo.mode = GPIO_MODE_INPUT;
+  io_conf_echo.pin_bit_mask = 1ULL << sensor->echoPin;
+  io_conf_echo.pull_down_en = GPIO_PULLDOWN_DISABLE;
+  io_conf_echo.pull_up_en = GPIO_PULLUP_ENABLE;
+  gpio_config(&io_conf_echo);
+
+  attachSensorInterrupt(idx);
+}
+
+/* Attach the interrupt handler to the pin that belongs to the sensor
+ * designated by `idx`. */
+void TOFManager::attachSensorInterrupt(uint8_t idx) {
+  gpio_isr_handler_add(sensors(idx)->echoPin, sensorInterrupt, sensors(idx));
+}
+
+/* Detach all interrupts for our sensors. */
+void TOFManager::detachInterrupts() {
+  for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i++) {
+    gpio_isr_handler_remove(sensors(i)->echoPin);
+  }
+}
+
+/* Disable the measurement on the sensor designated by `idx`. */
+void TOFManager::disableMeasurement(uint8_t idx) {
+  gpio_set_level(sensors(idx)->triggerPin, 0);
+}
diff --git a/src/tofmanager.h b/src/tofmanager.h
new file mode 100644
index 00000000..66dea26f
--- /dev/null
+++ b/src/tofmanager.h
@@ -0,0 +1,86 @@
+#ifndef TOFMANAGER_H
+#define TOFMANAGER_H
+
+#include <Arduino.h>
+#include <cstdint>
+#include <atomic>
+
+typedef struct {
+  uint8_t sensor_index; // the global index of the sensor this event occured on
+  uint32_t trigger;     // the microseconds since the sensor was triggered
+  uint32_t start;       // the microseconds since the sensor started the measurement
+  uint32_t end;         // the microseconds since the sensor started the measurement
+} LLMessage;
+
+/* low-level TOF sensor state (private interface)
+ *
+ * This structs holds the low level state of each sensor channel.
+ */
+typedef struct {
+  gpio_num_t triggerPin;
+  gpio_num_t echoPin;
+
+  /* timestamp when the trigger signal was sent in microseconds */
+  std::atomic_uint_least64_t trigger{0};
+  /* timestamp when the reading was started in microseconds */
+  std::atomic_uint_least64_t start{0};
+  /* timestamp when the reading was complete in microseconds
+   * if end == 0 - a measurement is in progress */
+  std::atomic_uint_least64_t end{0};
+
+  volatile uint32_t interrupt_count = 0;
+  uint32_t started_measurements = 0;
+  uint32_t successful_measurements = 0;
+  uint32_t lost_signals = 0;
+} LLSensor;
+
+// FIXME unify index parameter names
+
+/* low-level TOF sensor manager
+ *
+ * This class is responsible for all the low level plumbing: interrupts, timings, pins, etc
+ * This class deals exclusively with microseconds. The companion high level interface is
+ * DistanceSensorManager. These two parts of the code communicate via a FreeRTOS Queue.
+ */
+class TOFManager {
+  public:
+    void setupSensor(LLSensor*, uint8_t idx);
+    void detachInterrupts();
+    void attachInterrupts();
+
+    void loop();
+    void init();
+
+    /*
+     * startupManager()
+     *
+     * The only thing you ever need to call. This will spawn another task on a different CPU
+     * and you won't have to interact with this class at all.
+     *
+     * The LLManager will send you LLMessages on the sensor_queue you pass in.
+     */
+    static void startupManager(QueueHandle_t sensor_queue);
+  private:
+    TOFManager(QueueHandle_t q, LLSensor* sensors) : m_sensors(sensors), sensor_queue(q) {}
+    ~TOFManager(); // FIXME
+
+    void startMeasurement(uint8_t sensorId);
+    void disableMeasurement(uint8_t sensor_idx);
+    void attachSensorInterrupt(uint8_t idx);
+    boolean isSensorReady(uint8_t sensorId);
+    boolean isMeasurementComplete(uint8_t sensor_idx);
+
+    uint8_t primarySensor = 1;
+    volatile uint8_t currentSensor = 0;
+
+    void sendData(uint8_t sensor_idx);
+    static void sensorTaskFunction(void *parameter);
+
+    LLSensor* sensors(int i) {
+      return &m_sensors[i];
+    }
+    LLSensor* m_sensors;
+    QueueHandle_t sensor_queue;
+};
+
+#endif // TOFMANAGER_H
diff --git a/src/utils/median.h b/src/utils/median.h
index 95e06446..e36fa475 100644
--- a/src/utils/median.h
+++ b/src/utils/median.h
@@ -41,7 +41,6 @@ template<typename T> class Median {
       }
     };
     ~Median() {
-      log_i("Will free Median(%d/%d) -> 0x%lx", size, mid, data);
       delete[] data;
       delete[] temp;
     };