diff --git a/RotaxMonitor/.vscode/extensions.json b/RotaxMonitor/.vscode/extensions.json
index 080e70d..411655e 100644
--- a/RotaxMonitor/.vscode/extensions.json
+++ b/RotaxMonitor/.vscode/extensions.json
@@ -1,7 +1,7 @@
 {
-    // See http://go.microsoft.com/fwlink/?LinkId=827846
-    // for the documentation about the extensions.json format
     "recommendations": [
+        "Jason2866.esp-decoder",
+        "pioarduino.pioarduino-ide",
         "platformio.platformio-ide"
     ],
     "unwantedRecommendations": [
diff --git a/RotaxMonitor/boards/esp32-s3-n16r8.json b/RotaxMonitor/boards/esp32-s3-n16r8.json
index c0612bf..65002da 100644
--- a/RotaxMonitor/boards/esp32-s3-n16r8.json
+++ b/RotaxMonitor/boards/esp32-s3-n16r8.json
@@ -11,8 +11,8 @@
       "-DARDUINO_RUNNING_CORE=1",
       "-DARDUINO_EVENT_RUNNING_CORE=1",
       "-DBOARD_HAS_PSRAM",
-      "-DARDUINO_USB_MODE=1",
-      "-DARDUINO_USB_CDC_ON_BOOT=0"
+      "-DARDUINO_USB_MODE=0",
+      "-DARDUINO_USB_CDC_ON_BOOT=1"
     ],
     "f_cpu": "240000000L",
     "f_flash": "80000000L",
diff --git a/RotaxMonitor/platformio.ini b/RotaxMonitor/platformio.ini
index 63497eb..117d9d2 100644
--- a/RotaxMonitor/platformio.ini
+++ b/RotaxMonitor/platformio.ini
@@ -8,9 +8,9 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env:esp32-s3-n16r8]
-board = esp32-s3-n16r8
-platform = https://github.com/pioarduino/platform-espressif32/releases/download/55.03.37/platform-espressif32.zip
+[env:esp32-s3-devkitc1-n16r8]
+board = esp32-s3-devkitc1-n16r8
+platform = https://github.com/pioarduino/platform-espressif32/releases/download/stable/platform-espressif32.zip
 framework = arduino
 lib_deps = 
 	hideakitai/DebugLog@^0.8.4
@@ -18,51 +18,51 @@ lib_deps =
 	hideakitai/PCA95x5@^0.1.3
 	adafruit/Adafruit SSD1306@^2.5.16
 	garfius/Menu-UI@^1.2.0
-board_build.partitions = partitions/default_16MB.csv
-board_build.psram = enabled
-monitor_speed = 115200
+
+;Upload protocol configuration
+upload_protocol = esptool
+upload_port = /dev/ttyACM2
 upload_speed = 921600
+
+;Monitor configuration
+monitor_port = /dev/ttyACM2
+monitor_speed = 921600
+
+; Build configuration
 build_type = release
+build_flags = 
+    -DARDUINO_USB_CDC_ON_BOOT=0
+    -DARDUINO_USB_MODE=0
+    -fstack-protector-all
+    -DCONFIG_FREERTOS_GENERATE_RUN_TIME_STATS=1
+    -DCONFIG_FREERTOS_USE_TRACE_FACILITY=1
 
-[env:esp32-s3-n16r8-debug]
-board = ${env:esp32-s3-n16r8.board}
-platform = ${env:esp32-s3-n16r8.platform}
-framework = ${env:esp32-s3-n16r8.framework}
-lib_deps = ${env:esp32-s3-n16r8.lib_deps}
-board_build.partitions = partitions/default_16MB.csv
-board_build.psram = enabled
-monitor_speed = 115200
+[env:esp32-s3-devkitc1-n16r8-debug]
+board = ${env:esp32-s3-devkitc1-n16r8.board}
+platform = ${env:esp32-s3-devkitc1-n16r8.platform}
+framework = ${env:esp32-s3-devkitc1-n16r8.framework}
+lib_deps = ${env:esp32-s3-devkitc1-n16r8.lib_deps}
+
+;Upload protocol configuration
+upload_protocol = esptool
+upload_port = /dev/ttyACM2
 upload_speed = 921600
+
+;Monitor configuration
+monitor_port = /dev/ttyACM2
+monitor_speed = 921600
+
+; Debug configuration
+debug_tool = esp-builtin
+debug_speed = 15000
+
+; Build configuration
 build_type = debug
 build_flags = 
-	-O0
-	-g3
-	-ggdb
-	-fno-inline
-	-fno-ipa-sra
-	-fno-tree-sra
-	-fno-builtin
-
-
-[env:esp32-devtest-debug]
-board = esp32dev
-platform = https://github.com/pioarduino/platform-espressif32/releases/download/55.03.37/platform-espressif32.zip
-framework = arduino
-lib_deps = 
-	hideakitai/DebugLog@^0.8.4
-	bblanchon/ArduinoJson@^7.4.2
-	hideakitai/PCA95x5@^0.1.3
-	adafruit/Adafruit SSD1306@^2.5.16
-	garfius/Menu-UI@^1.2.0
-board_build.flash_size = 4MB
-board_build.partitions = default.csv
-monitor_speed = 115200
-build_type = debug
-build_flags = 
-	-O0
-	-g3
-	-ggdb
-	-fno-inline
-	-fno-ipa-sra
-	-fno-tree-sra
-	-fno-builtin
+    -O0
+    -g3
+    -ggdb3
+    -DCORE_DEBUG_LEVEL=5
+    -DARDUINO_USB_CDC_ON_BOOT=0
+    -DARDUINO_USB_MODE=0
+    -fstack-protector-all
diff --git a/RotaxMonitor/src/ADS1256.cpp b/RotaxMonitor/src/ADS1256.cpp
index 3372b47..79e2430 100644
--- a/RotaxMonitor/src/ADS1256.cpp
+++ b/RotaxMonitor/src/ADS1256.cpp
@@ -120,14 +120,14 @@ void ADS1256::setDRATE(uint8_t drate) //Setting DRATE (sampling frequency)
 {		
 	writeRegister(DRATE_REG, drate);
 	_DRATE = drate;
-	delay(200);
+	delayMicroseconds(500);
 }
 
 void ADS1256::setMUX(uint8_t mux) //Setting MUX (input channel)
 {	
 	writeRegister(MUX_REG, mux);
 	_MUX = mux;
-	delay(200);
+	//delayMicroseconds(500);
 }
 
 void ADS1256::setPGA(uint8_t pga) //Setting PGA (input voltage range)
@@ -138,7 +138,7 @@ void ADS1256::setPGA(uint8_t pga) //Setting PGA (input voltage range)
 	_ADCON = (_ADCON & 0b11111000) | (_PGA & 0b00000111); // Clearing and then setting bits 2-0 based on pga
 	
 	writeRegister(ADCON_REG, _ADCON);	
-	delay(200);
+	delayMicroseconds(1000); //Delay to allow the PGA to settle after changing its value
 	
 	updateConversionParameter(); //Update the multiplier according top the new PGA value
 }
@@ -501,8 +501,6 @@ void ADS1256::writeRegister(uint8_t registerAddress, uint8_t registerValueToWrit
   
   CS_HIGH();
   _spi->endTransaction();
-  delay(100);
-  
 }
 
 long ADS1256::readRegister(uint8_t registerAddress) //Reading a register
@@ -524,7 +522,7 @@ long ADS1256::readRegister(uint8_t registerAddress) //Reading a register
 
   CS_HIGH();
   _spi->endTransaction();
-  delay(100);
+
   return regValue;
 }
 
diff --git a/RotaxMonitor/src/devices.h b/RotaxMonitor/src/devices.h
index 869729f..e6027e5 100644
--- a/RotaxMonitor/src/devices.h
+++ b/RotaxMonitor/src/devices.h
@@ -1,5 +1,8 @@
 #pragma once
 
+// Library defines
+#define ADS1256_SPI_ALREADY_STARTED
+
 // Device Libraries
 #include <ADS1256.h>
 #include <AD5292.h>
diff --git a/RotaxMonitor/src/isr.cpp b/RotaxMonitor/src/isr.cpp
index 64c479e..a0c84c9 100644
--- a/RotaxMonitor/src/isr.cpp
+++ b/RotaxMonitor/src/isr.cpp
@@ -11,49 +11,47 @@ void trig_isr(void *arg)
     // exit if invalid args
     if (!arg)
         return;
-    // FOR TESTING ONLY    
-    digitalWrite(POT_A_CS, HIGH);
 
     BaseType_t xHigherPriorityTaskWoken = pdFALSE;
     isrParams *params = (isrParams *)arg;
     ignitionBoxStatus *box = params->ign_stat;
-    TaskHandle_t task_handle = *params->rt_handle_ptr;
+    TaskHandle_t task_handle = params->rt_handle_ptr;
 
     // exit if task not running
     if (!task_handle)
         return;
 
-    // reset spark flags, cannot be same time as trigger flags
-    box->coils12.spark_ok = false;
-    box->coils34.spark_ok = false;
-
     switch (params->flag)
     {
     case TRIG_FLAG_12P:
     case TRIG_FLAG_12N:
+        // only on first trigger to avoid multiple firing due to noise, to be fixed with hardware debounce
         box->coils12.trig_time = time_us;
         xTaskNotifyFromISR(task_handle, params->flag, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
         break;
     case TRIG_FLAG_34P:
     case TRIG_FLAG_34N:
+        // only on first trigger to avoid multiple firing due to noise, to be fixed with hardware debounce
         box->coils34.trig_time = time_us;
         xTaskNotifyFromISR(task_handle, params->flag, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
         break;
     case SPARK_FLAG_12:
+        box->coils34.spark_ok = false;
         box->coils12.spark_ok = true;
         box->coils12.spark_time = time_us;
-        vTaskNotifyGiveFromISR(task_handle, &xHigherPriorityTaskWoken);
+        xTaskNotifyFromISR(task_handle, params->flag, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
+        // vTaskNotifyGiveFromISR(task_handle, &xHigherPriorityTaskWoken);
         break;
     case SPARK_FLAG_34:
+        box->coils12.spark_ok = false;
         box->coils34.spark_ok = true;
         box->coils34.spark_time = time_us;
-        vTaskNotifyGiveFromISR(task_handle, &xHigherPriorityTaskWoken);
+        xTaskNotifyFromISR(task_handle, params->flag, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
+        // vTaskNotifyGiveFromISR(task_handle, &xHigherPriorityTaskWoken);
         break;
     default:
         break;
     }
-    // FOR TESTING ONLY
-    digitalWrite(POT_A_CS, LOW);
 
     if (xHigherPriorityTaskWoken)
         portYIELD_FROM_ISR();
diff --git a/RotaxMonitor/src/isr.h b/RotaxMonitor/src/isr.h
index 0dad9f9..c75e2c0 100644
--- a/RotaxMonitor/src/isr.h
+++ b/RotaxMonitor/src/isr.h
@@ -1,11 +1,12 @@
 #pragma once
 
 // Test device Flag
-#define TEST
+// #define TEST
 
 // Arduino Libraries
 #include <Arduino.h>
 #include "soc/gpio_struct.h"
+#include <map>
 #ifndef TEST
 #include "pins.h"
 #else
@@ -20,14 +21,15 @@
 // =====================
 // Event Flags (bitmask)
 // =====================
-#define TRIG_FLAG_12P (1 << 0)
-#define TRIG_FLAG_12N (1 << 2)
-#define TRIG_FLAG_34P (1 << 1)
-#define TRIG_FLAG_34N (1 << 3)
+static const uint32_t TRIG_FLAG_12P = (1 << 0);
+static const uint32_t TRIG_FLAG_12N = (1 << 1);
+static const uint32_t TRIG_FLAG_34P = (1 << 2);
+static const uint32_t TRIG_FLAG_34N = (1 << 3);
 
-#define SPARK_FLAG_NIL (1 << 8)
-#define SPARK_FLAG_12 (1 << 9)
-#define SPARK_FLAG_34 (1 << 10)
+static const uint32_t SPARK_FLAG_NIL = (1 << 8);
+static const uint32_t SPARK_FLAG_12 = (1 << 9);
+static const uint32_t SPARK_FLAG_34 = (1 << 10);
+static const uint32_t SPARK_FLAG_TIMEOUT = (1 << 11);
 
 // Spark Status
 enum sparkStatus
@@ -45,41 +47,66 @@ enum sparkStatus
     SPARK_SYNC_FAIL,
 };
 
+static const std::map<const sparkStatus, const char *> sparkStatusNames = {
+    {SPARK_POS_OK, "SPARK_POS_OK"},
+    {SPARK_NEG_OK, "SPARK_NEG_OK"},
+    {SPARK_POS_SKIP, "SPARK_POS_SKIP"},
+    {SPARK_NEG_SKIP, "SPARK_NEG_SKIP"},
+    {SPARK_POS_WAIT, "SPARK_POS_WAIT"},
+    {SPARK_NEG_WAIT, "SPARK_NEG_WAIT"},
+    {SPARK_POS_FAIL, "SPARK_POS_FAIL"},
+    {SPARK_NEG_FAIL, "SPARK_NEG_FAIL"},
+    {SPARK_POS_UNEXPECTED, "SPARK_POS_UNEXPECTED"},
+    {SPARK_NEG_UNEXPECTED, "SPARK_NEG_UNEXPECTED"},
+    {SPARK_SYNC_FAIL, "SPARK_SYNC_FAIL"},
+};
+
 enum softStartStatus
 {
     NORMAL,
-    SOFT_START
+    SOFT_START,
+    ERROR,
+};
+
+const std::map<const softStartStatus, const char *> softStartStatusNames = {
+    {NORMAL, "NORMAL"},
+    {SOFT_START, "SOFT_START"},
+    {ERROR, "ERROR"},
 };
 
 struct coilsStatus
 {
-    int64_t trig_time;
-    int64_t spark_time;
-    int64_t spark_delay;
-    sparkStatus spark_status;
-    softStartStatus sstart_status;
-    float peak_p_in, peak_n_in;
-    float peak_p_out, peak_n_out;
-    float trigger_spark;
-    bool spark_ok;
+    int64_t trig_time = 0;
+    int64_t spark_time = 0;
+    int64_t spark_delay = 0; // in microseconds
+    sparkStatus spark_status = sparkStatus::SPARK_POS_OK;
+    softStartStatus sstart_status = softStartStatus::NORMAL;
+    float peak_p_in = 0.0, peak_n_in = 0.0;
+    float peak_p_out = 0.0, peak_n_out = 0.0;
+    float trigger_spark = 0.0;
+    bool spark_ok = false;
+    uint32_t n_events = 0;
+
 };
 
 // Task internal Status
 struct ignitionBoxStatus
 {
-    int64_t timestamp;
+    int64_t timestamp = 0;
     // coils pairs for each ignition
     coilsStatus coils12;
     coilsStatus coils34;
     // voltage from generator
     float volts_gen = 0.0;
+    uint32_t n_queue_errors = 0;
+    uint32_t adc_read_time = 0;
 };
 
-struct isrParams {
+struct isrParams
+{
     const uint32_t flag;
-    ignitionBoxStatus* ign_stat;
-    TaskHandle_t* rt_handle_ptr;
+    ignitionBoxStatus *ign_stat;
+    TaskHandle_t rt_handle_ptr;
 };
 
-
 void IRAM_ATTR trig_isr(void *arg);
diff --git a/RotaxMonitor/src/main.cpp b/RotaxMonitor/src/main.cpp
index 6001f01..ef60521 100644
--- a/RotaxMonitor/src/main.cpp
+++ b/RotaxMonitor/src/main.cpp
@@ -12,18 +12,29 @@
 #include <devices.h>
 #include <ui.h>
 
-void printTaskList()
+// FreeRTOS directives
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+// #define CH_B_ENABLE
+#define TEST
+
+float freqToRPM(float freq)
+{
+    return freq * 60.0f; // 1 pulse per revolution
+}
+
+void printTaskStats()
 {
     char buffer[1024];
-    Serial.println("Task Name\tState\tPrio\tStack\tNum");
-    vTaskList(buffer);
+    vTaskGetRunTimeStats(buffer);
     Serial.println(buffer);
 }
 
 void setup()
 {
+    Serial.begin(921600);
     delay(250);
-    Serial.begin(115200);
 
     // Setup Logger
     LOG_ATTACH_SERIAL(Serial);
@@ -51,15 +62,15 @@ void loop()
 {
     // global variables
     bool running = true;
-    Devices dev;
+    static Devices dev;
 
     // Task handle
-    TaskHandle_t trigA_TaskHandle = NULL;
-    TaskHandle_t trigB_TaskHandle = NULL;
+    static TaskHandle_t trigA_TaskHandle = NULL;
+    static TaskHandle_t trigB_TaskHandle = NULL;
 
-    QueueHandle_t rt_taskA_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
-    QueueHandle_t rt_taskB_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
-    rtTaskParams taskA_params{
+    static QueueHandle_t rt_taskA_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
+    static QueueHandle_t rt_taskB_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
+    static rtTaskParams taskA_params{
         .rt_running = true,
         .dev = &dev,
         .rt_handle_ptr = &trigA_TaskHandle,
@@ -75,13 +86,14 @@ void loop()
         .rt_resets = rtTaskResets{.rst_io_12p = RST_EXT_A12P, .rst_io_12n = RST_EXT_A12N, .rst_io_34p = RST_EXT_A34P, .rst_io_34n = RST_EXT_A34N}};
 
     LOG_INFO("Task Variables OK");
-#ifndef TEST
+
+#ifdef CH_B_ENABLE
     QueueHandle_t rt_taskB_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
     rtTaskParams taskB_params{
         .rt_running = true,
         .dev = &dev,
-        .rt_queue = rt_taskB_queue,
         .rt_handle_ptr = &trigB_TaskHandle,
+        .rt_queue = rt_taskB_queue,
         .rt_int = rtTaskInterrupts{
             .isr_ptr = trig_isr,
             .trig_pin_12p = TRIG_PIN_B12P,
@@ -95,13 +107,16 @@ void loop()
 
     bool spiA_ok = true;
     bool spiB_ok = true;
-#ifndef TEST
+
     // Init 2 SPI interfaces
     SPIClass SPI_A(FSPI);
     spiA_ok = SPI_A.begin(SPI_A_SCK, SPI_A_MISO, SPI_A_MOSI);
+    SPI_A.setDataMode(SPI_MODE1); // ADS1256 requires SPI mode 1
+    #ifndef TEST
     SPIClass SPI_B(HSPI);
     spiB_ok = SPI_B.begin(SPI_B_SCK, SPI_B_MISO, SPI_B_MOSI);
-#endif
+    SPI_B.setDataMode(SPI_MODE1); // ADS1256 requires SPI mode 1
+    #endif
     if (!spiA_ok || !spiB_ok)
     {
         LOG_ERROR("Unable to Initialize SPI Busses");
@@ -111,13 +126,13 @@ void loop()
     }
     LOG_INFO("Init SPI OK");
 
-#ifndef TEST
     // Init ADC_A
-    dev.adc_a = new ADS1256(ADC_A_DRDY, ADC_A_RST, ADC_A_SYNC, ADC_A_CS, 2.5, &SPI_A);
+    dev.adc_a = new ADS1256(ADC_A_DRDY, ADS1256::PIN_UNUSED, ADC_A_SYNC, ADC_A_CS, 2.5, &SPI_A);
     dev.adc_a->InitializeADC();
     dev.adc_a->setPGA(PGA_1);
-    dev.adc_a->setDRATE(DRATE_1000SPS);
+    dev.adc_a->setDRATE(DRATE_7500SPS);
 
+#ifndef TEST
     // Init ADC_B
     dev.adc_a = new ADS1256(ADC_B_DRDY, ADC_B_RST, ADC_B_SYNC, ADC_B_CS, 2.5, &SPI_B);
     dev.adc_a->InitializeADC();
@@ -140,7 +155,7 @@ void loop()
 
     // Ignition B on Core 1
     auto ignB_task_success = pdPASS;
-#ifndef TEST
+#ifdef CH_B_ENABLE
     ignB_task_success = xTaskCreatePinnedToCore(
         rtIgnitionTask,
         "rtIgnitionTask_boxB",
@@ -162,49 +177,62 @@ void loop()
     LOG_INFO("Real Time Tasks A & B initialized");
 
     ////////////////////// MAIN LOOP //////////////////////
-    uint32_t count(0);
     clearScreen();
     setCursor(0, 0);
+    ignitionBoxStatus ignA;
+    int64_t last = esp_timer_get_time();
+    uint32_t missed_firings12 = 0;
+    uint32_t missed_firings34 = 0;
+    uint32_t counter = 0;
 
     while (running)
     {
-        ignitionBoxStatus ignA;
-        if (xQueueReceive(rt_taskA_queue, &ignA, pdMS_TO_TICKS(100)) == pdTRUE)
+        if (xQueueReceive(rt_taskA_queue, &ignA, pdMS_TO_TICKS(1000)) == pdTRUE)
         {
-            printField("++ Timestamp", (uint32_t)ignA.timestamp, 0, 0);
-            if (firstRun)
-                Serial.println("========== Coils 12 =============");
-            printField("Pickup Tim", (uint32_t)ignA.coils12.trig_time, 0, 1);
-            printField("Spark Tim", (uint32_t)ignA.coils12.spark_time, 0, 2);
-            printField("Spark Dly", (uint32_t)ignA.coils12.spark_delay, 0, 3);
-            printField("Spark Sts", (uint32_t)ignA.coils12.spark_status, 0, 4);
-            printField("Peak P_IN", ignA.coils12.peak_p_in, 0, 5);
-            printField("Peak P_OUT", ignA.coils12.peak_p_out, 0, 6);
-            printField("Peak N_IN", ignA.coils12.peak_n_in, 0, 7);
-            printField("Peak N_OUT", ignA.coils12.peak_n_out, 0, 8);
-            printField("SoftStart ", (uint32_t)ignA.coils12.sstart_status, 0, 9);
-            if (firstRun)
-                Serial.println("========== Coils 34 =============");
-            printField("Pickup Tim", (uint32_t)ignA.coils34.trig_time, 0, 11);
-            printField("Spark Tim", (uint32_t)ignA.coils34.spark_time, 0, 12);
-            printField("Spark Dly", (uint32_t)ignA.coils34.spark_delay, 0, 13);
-            printField("Spark Sts", (uint32_t)ignA.coils34.spark_delay, 0, 14);
-            printField("Peak P_IN", ignA.coils34.peak_p_in, 0, 15);
-            printField("Peak P_OUT", ignA.coils34.peak_p_out, 0, 16);
-            printField("Peak N_IN", ignA.coils34.peak_n_in, 0, 17);
-            printField("Peak N_OUT", ignA.coils34.peak_n_out, 0, 18);
-            printField("SoftStart ", (uint32_t)ignA.coils34.sstart_status, 0, 19);
-            if (firstRun)
-                Serial.println("========== END =============");
-            if (count++ % 10 == 0)
-            {
-                firstRun = true;
-                clearScreen();
-                setCursor(0, 0);
-            }
-            else
+            float freq = (esp_timer_get_time() - last) / 1000000.0f; // in seconds
+            freq = freq > 0 ? 1.0f / freq : 0;                      // Calculate frequency (Hz)
+            last = esp_timer_get_time();
 
-                firstRun = false;
+            if (ignA.coils12.spark_status == sparkStatus::SPARK_POS_FAIL || ignA.coils12.spark_status == sparkStatus::SPARK_NEG_FAIL)
+                missed_firings12++;
+            if (ignA.coils34.spark_status == sparkStatus::SPARK_POS_FAIL || ignA.coils34.spark_status == sparkStatus::SPARK_NEG_FAIL)
+                missed_firings34++;
+
+            clearScreen();
+            setCursor(0, 0);
+            printField("++ Timestamp", (uint32_t)ignA.timestamp);
+            Serial.println("========== Coils 12 =============");
+            printField("Events", (uint32_t)ignA.coils12.n_events);
+            printField("Missed Firing", missed_firings12);
+            printField("Spark Dly", (uint32_t)ignA.coils12.spark_delay);
+            printField("Spark Sts", sparkStatusNames.at(ignA.coils12.spark_status));
+            printField("Peak P_IN", ignA.coils12.peak_p_in);
+            printField("Peak N_IN", ignA.coils12.peak_n_in);
+            printField("Peak P_OUT", ignA.coils12.peak_p_out);
+            printField("Peak N_OUT", ignA.coils12.peak_n_out);
+            printField("Soft Start ", softStartStatusNames.at(ignA.coils12.sstart_status));
+
+            Serial.println("========== Coils 34 =============");
+            printField("Events", (uint32_t)ignA.coils34.n_events);
+            printField("Missed Firing", missed_firings34);
+            printField("Spark Dly", (uint32_t)ignA.coils34.spark_delay);
+            printField("Spark Sts", sparkStatusNames.at(ignA.coils34.spark_status));
+            printField("Peak P_IN", ignA.coils34.peak_p_in);
+            printField("Peak N_IN", ignA.coils34.peak_n_in);
+            printField("Peak P_OUT", ignA.coils34.peak_p_out);
+            printField("Peak N_OUT", ignA.coils34.peak_n_out);
+            printField("Soft Start ", softStartStatusNames.at(ignA.coils34.sstart_status));
+
+            Serial.println("========== END =============");
+            Serial.println();
+            printField("Engine RPM", freqToRPM(freq));
+            printField("ADC Read Time", (uint32_t)ignA.adc_read_time);
+            printField("Queue Errors", (uint32_t)ignA.n_queue_errors);
+        }
+        else
+        {
+            Serial.println("Waiting for data... ");
+            delay(500);
         }
     }
 
diff --git a/RotaxMonitor/src/pins.h b/RotaxMonitor/src/pins.h
index de0bec2..62a3ded 100644
--- a/RotaxMonitor/src/pins.h
+++ b/RotaxMonitor/src/pins.h
@@ -48,27 +48,25 @@
 // =====================
 #define ADC_A_CS     4
 #define ADC_A_DRDY   5
-#define ADC_A_RST    6
-#define ADC_A_SYNC   7
+#define ADC_A_SYNC   6
 
 #define ADC_B_CS     14
 #define ADC_B_DRDY   15
-#define ADC_B_RST    16
-#define ADC_B_SYNC   17
+#define ADC_B_SYNC   16
 
 // =====================
 // DIGITAL POT
 // =====================
-#define POT_A_CS    1
-#define POT_B_CS    2
+#define POT_A_CS    7
+#define POT_B_CS    17
 
 // =====================
 // TRIGGER INPUT INTERRUPTS
 // =====================
 #define TRIG_PIN_A12P   18
 #define TRIG_PIN_A12N   21
-#define TRIG_PIN_A34P   33
-#define TRIG_PIN_A34N   34
+#define TRIG_PIN_A34P   1
+#define TRIG_PIN_A34N   2
 #define TRIG_PIN_B12P   38
 #define TRIG_PIN_B12N   39
 #define TRIG_PIN_B34P   40
@@ -78,8 +76,8 @@
 // SPARK DETECT INPUTS
 // =====================
 #define SPARK_PIN_A12   42
-#define SPARK_PIN_A34   45   // OK (strapping ma consentito)
-#define SPARK_PIN_B12   46   // OK (strapping ma consentito)
+#define SPARK_PIN_A34   45   // OK (strapping ma consentito) 45
+#define SPARK_PIN_B12   46   // OK (strapping ma consentito) 46
 #define SPARK_PIN_B34   47
 
 // =====================
diff --git a/RotaxMonitor/src/tasks.cpp b/RotaxMonitor/src/tasks.cpp
index 7e8349b..8a72bc0 100644
--- a/RotaxMonitor/src/tasks.cpp
+++ b/RotaxMonitor/src/tasks.cpp
@@ -1,4 +1,11 @@
 #include "tasks.h"
+#include <esp_timer.h>
+
+// Timeout callback for microsecond precision
+void spark_timeout_callback(void* arg) {
+    TaskHandle_t handle = (TaskHandle_t)arg;
+    xTaskNotify(handle, SPARK_FLAG_TIMEOUT, eSetValueWithOverwrite);
+}
 
 void rtIgnitionTask(void *pvParameters)
 {
@@ -16,13 +23,14 @@ void rtIgnitionTask(void *pvParameters)
     const rtTaskInterrupts rt_int = params->rt_int; // copy to avoid external override
     const rtTaskResets rt_rst = params->rt_resets;  // copy to avoid external override
     QueueHandle_t rt_queue = params->rt_queue;
-    TaskHandle_t *rt_handle_ptr = params->rt_handle_ptr;
+    TaskHandle_t rt_handle_ptr = *params->rt_handle_ptr;
     Devices *dev = params->dev;
     ADS1256 *adc = dev->adc_a;
     PCA9555 *io = dev->io;
 
+    ignitionBoxStatus ign_box_sts;
+
     // Variables for ISR, static to be fixed in memory locations
-    static ignitionBoxStatus ign_box_sts; // common for all ISR calls
     static isrParams isr_params_t12p{
         .flag = TRIG_FLAG_12P,
         .ign_stat = &ign_box_sts,
@@ -47,10 +55,18 @@ void rtIgnitionTask(void *pvParameters)
         .flag = SPARK_FLAG_34,
         .ign_stat = &ign_box_sts,
         .rt_handle_ptr = rt_handle_ptr};
-
+        
     LOG_INFO("rtTask ISR Params OK");
 
-    pinMode(POT_A_CS, OUTPUT);
+    // Create esp_timer for microsecond precision timeout
+    esp_timer_handle_t timeout_timer;
+    esp_timer_create_args_t timer_args = {
+        .callback = spark_timeout_callback,
+        .arg = (void*)rt_handle_ptr,
+        .dispatch_method = ESP_TIMER_TASK,
+        .name = "spark_timeout"
+    };
+    esp_timer_create(&timer_args, &timeout_timer);
 
     // Attach Pin Interrupts
     attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_12p), rt_int.isr_ptr, (void *)&isr_params_t12p, RISING);
@@ -70,8 +86,9 @@ void rtIgnitionTask(void *pvParameters)
 
     LOG_WARN("rtTask Init Correct");
     // Global rt_task_ptr variables
-    uint32_t it = 0;
-    uint32_t q_fail_count = 0;
+    bool first_cycle = true;
+    bool cycle12 = false;
+    bool cycle34 = false;
 
     while (params->rt_running)
     {
@@ -80,19 +97,25 @@ void rtIgnitionTask(void *pvParameters)
 
         // WAIT FOR PICKUP SIGNAL
         xTaskNotifyWait(
-            ULONG_MAX,    // non pulire all'ingresso
+            0x00,         // non pulire all'ingresso
             ULONG_MAX,    // pulisci i primi 8 bit
             &pickup_flag, // valore ricevuto
             portMAX_DELAY);
+        
+        if (first_cycle && pickup_flag != TRIG_FLAG_12P) // skip first cycle because of possible initial noise on pickup signals at startu
+        {
+            continue;
+        }
 
 #ifdef DEBUG
-        LOG_INFO("Iteration [", it++, "]");
         Serial.print("\033[2J"); // clear screen
         Serial.print("\033[H");  // cursor home
-        LOG_INFO("Pickup Flags: ", printBits(pickup_flag).c_str());
+        LOG_INFO("Iteration [", it++, "]");
+
         if (!names.contains(pickup_flag))
         {
             LOG_ERROR("Wrong Pickup Flag");
+            LOG_ERROR("Pickup Flags: ", printBits(pickup_flag).c_str());
             continue;
         }
         else
@@ -101,37 +124,50 @@ void rtIgnitionTask(void *pvParameters)
         }
 #endif
 
-        // WAIT FOR SPARK TO HAPPEN
-        auto spark_timeout = ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(spark_timeout_max));
-        if (ign_box_sts.coils12.spark_ok || ign_box_sts.coils34.spark_ok) // otherwise timeout if none is set in the ISR
-            spark_flag = ign_box_sts.coils12.spark_ok ? SPARK_FLAG_12 : SPARK_FLAG_34;
-        else
-            spark_flag == SPARK_FLAG_NIL;
+        // Start microsecond precision timeout timer
+        esp_timer_stop(timeout_timer); // stop timer in case it was running from previous cycle
+        esp_timer_start_once(timeout_timer, spark_timeout_max);
 
+        spark_flag = SPARK_FLAG_NIL; // default value in case of timeout, to be set by ISR if spark event occours
+        // WAIT FOR SPARK TO HAPPEN OR TIMEOUT
+        BaseType_t sp = pdFALSE;
+        sp = xTaskNotifyWait(
+            0x00,         // non pulire all'ingresso
+            ULONG_MAX,    // pulisci i primi 8 bit
+            &spark_flag, // valore ricevuto
+            portMAX_DELAY); // wait indefinitely, timeout handled by esp_timer
+
+        // Handle timeout or spark event
+        if (spark_flag == SPARK_FLAG_TIMEOUT) {
+            spark_flag = SPARK_FLAG_NIL;
+        } else {
+            // Spark occurred, stop the timer
+            esp_timer_stop(timeout_timer);
+        }
+
+#ifdef DEBUG
+        // LOG_INFO("Spark Flags: ", printBits(spark_flag).c_str());
+        LOG_INFO("Spark12:", ign_box_sts.coils12.spark_ok ? "TRUE" : "FALSE");
+        LOG_INFO("Spark34:", ign_box_sts.coils34.spark_ok ? "TRUE" : "FALSE");
+        if (names.contains(spark_flag))
+            LOG_INFO("Spark Trigger:", names.at(spark_flag));
+#endif
         xTaskNotifyStateClear(NULL);
         ulTaskNotifyValueClear(NULL, 0xFFFFFFFF);
 
-#ifdef DEBUG
-        LOG_INFO("Spark Flags: ", printBits(spark_flag).c_str());
-        if (!names.contains(spark_flag))
-            LOG_ERROR("No Spark");
-        else
-            LOG_INFO("Spark Trigger:", names.at(spark_flag));
-#endif
-
         // A trigger from pickup 12 is followed by a spark event on 34 or vice versa pickup 34 triggers spark on 12
-        if ((pickup_flag == TRIG_FLAG_12P || pickup_flag == TRIG_FLAG_12N) && spark_flag != SPARK_FLAG_12)
+        if ((pickup_flag == TRIG_FLAG_12P || pickup_flag == TRIG_FLAG_12N) && (spark_flag != SPARK_FLAG_12 && spark_flag != SPARK_FLAG_NIL))
         {
             ign_box_sts.coils12.spark_status = ign_box_sts.coils34.spark_status = sparkStatus::SPARK_SYNC_FAIL;
-            // Save error on circular buffer and skip to next cycle //
-            LOG_ERROR("Spark Mismatch");
             continue;
         }
 
+        // Select coil status reference based on pickup_flag
         coilsStatus *coils;
         switch (pickup_flag)
         {
         case TRIG_FLAG_12P:
+            first_cycle = false;
         case TRIG_FLAG_12N:
             coils = &ign_box_sts.coils12;
             break;
@@ -141,7 +177,7 @@ void rtIgnitionTask(void *pvParameters)
             break;
         }
 
-        bool new_data = false;
+        // Select logic based on pickup and spark flags
         switch (pickup_flag)
         {
         case TRIG_FLAG_12P:
@@ -154,8 +190,8 @@ void rtIgnitionTask(void *pvParameters)
                 coils->sstart_status = softStartStatus::NORMAL;  // because spark on positive edge
                 coils->spark_status = sparkStatus::SPARK_POS_OK; // do not wait for spark on negative edge
 #ifdef DEBUG
-                LOG_INFO("Trigger Spark POSITIVE");
-                LOG_INFO("Spark12 Delay Timer: ", (int)coils->spark_delay);
+                LOG_INFO("Spark on POSITIVE pulse");
+                LOG_INFO("Spark Delay Time: ", (int32_t)coils->spark_delay);
 #endif
             }
             // Timeout occourred, expected POSITIVE edge spark NOT OCCOURRED
@@ -164,39 +200,42 @@ void rtIgnitionTask(void *pvParameters)
                 coils->spark_status = sparkStatus::SPARK_NEG_WAIT;
                 coils->sstart_status = softStartStatus::NORMAL;
             }
-            new_data = true;
-            break; // Do nothing more on positive pulse
+            continue; // Do nothing more on positive pulse
         }
         // CASES for NEGATIVE cycle triggering of pickup and sparks 12 & 34
         case TRIG_FLAG_12N:
         case TRIG_FLAG_34N:
         {
-            const bool expected_negative12 = coils->spark_status == sparkStatus::SPARK_NEG_WAIT;
+            const bool expected_negative = coils->spark_status == sparkStatus::SPARK_NEG_WAIT;
             // Timeout not occourred, expected NEGATIVE edge spark OCCOURRED
-            if (spark_flag != SPARK_FLAG_NIL && expected_negative12)
+            if (spark_flag != SPARK_FLAG_NIL && expected_negative)
             {
                 coils->spark_delay = coils->spark_time - coils->trig_time;
                 coils->sstart_status = softStartStatus::SOFT_START;
-                coils->spark_status == sparkStatus::SPARK_NEG_OK;
+                coils->spark_status = sparkStatus::SPARK_NEG_OK;
 #ifdef DEBUG
-                LOG_INFO("Trigger Spark NEGATIVE");
-                LOG_INFO("Spark12 Delay Timer: ", (int)ign_box_sts.coils12.spark_delay);
+                LOG_INFO("Spark on NEGATIVE pulse");
+                LOG_INFO("Spark Delay Time: ", (int32_t)coils->spark_delay);
 #endif
             }
             // Timeout occourred, expected POSITIVE edge spark NOT OCCOURRED
-            else if (spark_flag == SPARK_FLAG_NIL && expected_negative12)
+            else if (spark_flag == SPARK_FLAG_NIL && expected_negative)
             {
-                coils->sstart_status = softStartStatus::NORMAL;
+                coils->sstart_status = softStartStatus::ERROR;
                 coils->spark_status = sparkStatus::SPARK_NEG_FAIL;
             }
             // Timeout not occouured, unexpected negative edge spark
-            else if (spark_flag != SPARK_FLAG_NIL && !expected_negative12)
+            else if (spark_flag != SPARK_FLAG_NIL && !expected_negative)
             {
                 coils->sstart_status = softStartStatus::SOFT_START;
                 coils->spark_status = sparkStatus::SPARK_NEG_UNEXPECTED;
             }
             // Wait for finish of negative pulse to save data to buffer
-            new_data = true;
+            coils->n_events++;
+            if (pickup_flag == TRIG_FLAG_12N)
+                cycle12 = true;
+            else
+                cycle34 = true;
             break;
         }
         default:
@@ -208,12 +247,14 @@ void rtIgnitionTask(void *pvParameters)
             break;
         }
 
-        if (new_data)
+        if (cycle12 && cycle34) // wait for both 12 and 34 cycles to complete before sending data to main loop and resetting peak detectors
         {
-            vTaskDelay(pdMS_TO_TICKS(1)); // delay 1ms to allow peak detectors to charge for negative cycle
-            // read adc channels: pickup12, out12 [ pos + neg ]
+            cycle12 = false;
+            cycle34 = false;
+            //  read adc channels: pickup12, out12 [ pos + neg ]
             if (adc) // read only if adc initialized
             {
+                uint32_t start_adc_read = esp_timer_get_time();
                 // from peak detector circuits
                 ign_box_sts.coils12.peak_p_in = adcReadChannel(adc, ADC_CH_PEAK_12P_IN);
                 ign_box_sts.coils12.peak_n_in = adcReadChannel(adc, ADC_CH_PEAK_12N_IN);
@@ -223,9 +264,10 @@ void rtIgnitionTask(void *pvParameters)
                 ign_box_sts.coils12.peak_n_out = adcReadChannel(adc, ADC_CH_PEAK_12N_OUT);
                 ign_box_sts.coils34.peak_p_out = adcReadChannel(adc, ADC_CH_PEAK_34P_OUT);
                 ign_box_sts.coils34.peak_n_out = adcReadChannel(adc, ADC_CH_PEAK_34N_OUT);
+                ign_box_sts.adc_read_time = (uint32_t)(esp_timer_get_time() - start_adc_read);
             }
             else // simulate adc read timig
-                vTaskDelay(pdMS_TO_TICKS(6));
+                vTaskDelay(pdMS_TO_TICKS(1));
 
             // reset peak detectors + sample and hold
             // outputs on io expander
@@ -237,18 +279,22 @@ void rtIgnitionTask(void *pvParameters)
                 io->write(iostat & ~rst_bitmask);
             }
             else
-                vTaskDelay(pdMS_TO_TICKS(2));
+                vTaskDelay(pdMS_TO_TICKS(1));
 
             // send essage to main loop with ignition info, by copy so local static variable is ok
             if (rt_queue)
                 ign_box_sts.timestamp = esp_timer_get_time(); // update data timestamp
-            if (xQueueSendToBack(rt_queue, (void *)&ign_box_sts, pdMS_TO_TICKS(1)) != pdPASS)
+            if (xQueueSendToBack(rt_queue, (void *)&ign_box_sts, 0) != pdPASS)
             {
-                q_fail_count++;
+                static uint32_t n_errors = 0;
+                n_errors++;
+                ign_box_sts.n_queue_errors = n_errors;
                 LOG_ERROR("Failed to send to rt_queue");
             }
         }
     }
+    // Delete the timeout timer
+    esp_timer_delete(timeout_timer);
     LOG_WARN("Ending realTime Task");
     // Ignition A Interrupts DETACH
     detachInterrupt(rt_int.trig_pin_12p);
diff --git a/RotaxMonitor/src/tasks.h b/RotaxMonitor/src/tasks.h
index d10b840..c1196eb 100644
--- a/RotaxMonitor/src/tasks.h
+++ b/RotaxMonitor/src/tasks.h
@@ -2,7 +2,7 @@
 #define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG
 
 // Serial debug flag
-// #define DEBUG
+//#define DEBUG
 
 // Arduino Libraries
 #include <Arduino.h>
@@ -16,7 +16,7 @@
 #include "devices.h"
 
 // Global Variables and Flags
-const uint8_t spark_timeout_max = 2; // in milliseconds
+const uint32_t spark_timeout_max = 500; // in microseconds
 
 // Debug Variables
 #ifdef DEBUG
@@ -27,6 +27,7 @@ static const std::map<const uint32_t, const char *> names = {
     {TRIG_FLAG_34N, "TRIG_FLAG_34N"},
     {SPARK_FLAG_12, "SPARK_FLAG_12"},
     {SPARK_FLAG_34, "SPARK_FLAG_34"},
+    {SPARK_FLAG_TIMEOUT, "SPARK_FLAG_TIMEOUT"},
 };
 #endif
 
diff --git a/RotaxMonitor/src/ui.h b/RotaxMonitor/src/ui.h
index bf7f793..9c801be 100644
--- a/RotaxMonitor/src/ui.h
+++ b/RotaxMonitor/src/ui.h
@@ -2,47 +2,35 @@
 
 #include <Arduino.h>
 
-static bool firstRun = true;
-
-void clearScreen(){
+void clearScreen()
+{
     Serial.print("\033[2J"); // clear screen
     Serial.print("\033[H");  // cursor home
     Serial.flush();
 }
 
-void setCursor(const uint8_t x, const uint8_t y) {
-    Serial.printf("\033[%d;%d", y, x+1);
+void setCursor(const uint8_t x, const uint8_t y)
+{
+    Serial.printf("\033[%d;%d", y, x + 1);
     Serial.flush();
 }
 
-void printField(const char name[], const uint32_t val, const uint8_t x, const uint8_t y) {
-    if (firstRun) {
-        setCursor(x,y);
-        Serial.printf("%15s: %06d\n", name, val);
-        return;
-    }
-    setCursor(x+16, y);
-    Serial.print(val);
+void printField(const char name[], const uint32_t val)
+{
+    Serial.printf("%15s: %06d\n", name, val);
 }
 
-void printField(const char name[], const int64_t val, const uint8_t x, const uint8_t y) {
-    if (firstRun) {
-        setCursor(x,y);
-        Serial.printf("%15s: %06u\n", name, (uint64_t)val);
-        return;
-    }
-    setCursor(x+16, y);
-    Serial.print((uint64_t)val);
-    Serial.flush();
+void printField(const char name[], const int64_t val)
+{
+    Serial.printf("%15s: %06u\n", name, (uint64_t)val);
 }
 
-void printField(const char name[], const float val, const uint8_t x, const uint8_t y) {
-    if (firstRun) {
-        setCursor(x,y);
-        Serial.printf("%15s: %4.2f\n", name, val);
-        return;
-    }
-    setCursor(x+16, y);
-    Serial.print(val);
-    Serial.flush();
+void printField(const char name[], const float val)
+{
+    Serial.printf("%15s: %4.2f\n", name, val);
+}
+
+void printField(const char name[], const char *val)
+{
+    Serial.printf("%15s: %s\n", name, val);
 }
\ No newline at end of file
diff --git a/RotaxMonitorTester/.vscode/extensions.json b/RotaxMonitorTester/.vscode/extensions.json
index 080e70d..411655e 100644
--- a/RotaxMonitorTester/.vscode/extensions.json
+++ b/RotaxMonitorTester/.vscode/extensions.json
@@ -1,7 +1,7 @@
 {
-    // See http://go.microsoft.com/fwlink/?LinkId=827846
-    // for the documentation about the extensions.json format
     "recommendations": [
+        "Jason2866.esp-decoder",
+        "pioarduino.pioarduino-ide",
         "platformio.platformio-ide"
     ],
     "unwantedRecommendations": [
diff --git a/RotaxMonitorTester/platformio.ini b/RotaxMonitorTester/platformio.ini
index 8fb436c..2653613 100644
--- a/RotaxMonitorTester/platformio.ini
+++ b/RotaxMonitorTester/platformio.ini
@@ -10,24 +10,24 @@
 
 [env:esp32-devtest-release]
 board = esp32dev
-platform = https://github.com/pioarduino/platform-espressif32/releases/download/55.03.37/platform-espressif32.zip
+platform = https://github.com/pioarduino/platform-espressif32/releases/download/stable/platform-espressif32.zip
 framework = arduino
 lib_deps = 
 	hideakitai/DebugLog@^0.8.4
 board_build.flash_size = 4MB
 board_build.partitions = default.csv
-monitor_speed = 115200
+monitor_speed = 921600
 build_type = release
 
 [env:esp32-devtest-debug]
 board = esp32dev
-platform = https://github.com/pioarduino/platform-espressif32/releases/download/55.03.37/platform-espressif32.zip
-framework = arduino
+platform = https://github.com/pioarduino/platform-espressif32/releases/download/stable/platform-espressif32.zip
+
 lib_deps = 
 	hideakitai/DebugLog@^0.8.4
 board_build.flash_size = 4MB
 board_build.partitions = default.csv
-monitor_speed = 115200
+monitor_speed = 921600
 build_type = debug
 build_flags = 
 	-O0
diff --git a/RotaxMonitorTester/src/main.cpp b/RotaxMonitorTester/src/main.cpp
index 1985d2f..ea657e7 100644
--- a/RotaxMonitorTester/src/main.cpp
+++ b/RotaxMonitorTester/src/main.cpp
@@ -15,6 +15,20 @@ static uint32_t count = 0;
 #define SPARK_DLY_MIN 10
 #define SPARK_DLY_MAX 490
 
+#define PAUSE_LONG_MIN 5000
+#define PAUSE_LONG_MAX PAUSE_LONG_MIN*100
+
+#define RPM_MIN 800
+#define RPM_MAX 5500
+
+void clearScreen(){
+    Serial.print("\033[2J"); // clear screen
+    Serial.print("\033[H");  // cursor home
+    Serial.flush();
+}
+
+static double filtered_rpm = 0;
+
 static const std::map<const uint32_t, const char *> pin2Name = {
     {PIN_TRIG_A12P, "HIGH_PIN_TRIG_A12P"},
     {~PIN_TRIG_A12P, "LOW_PIN_TRIG_A12P"},
@@ -35,15 +49,16 @@ static timerStatus stsA = {
       .clock_period_us = (uint32_t)PERIOD_US,
       .pause_long_us = 10000,
       .pause_short_us = 1000,
-      .coil_pulse_us = 500,
-      .spark_pulse_us = 50,
-      .spark_delay_us = 10,
+      .coil_pulse_us = 1000,
+      .spark_pulse_us = 100,
+      .spark_delay_us = 50,
       .main_task = NULL};
 
 void setup()
 {
 
-  Serial.begin(115200);
+  Serial.begin(921600);
+  delay(1000);
   LOG_ATTACH_SERIAL(Serial);
 
   pinMode(PIN_TRIG_A12P, OUTPUT);
@@ -81,6 +96,16 @@ void loop()
   } else {
     stsA.soft_start = false;
   }
+
+  double new_rpm = (double)(map(analogRead(FREQ_POT), 0, 4096, RPM_MIN, RPM_MAX));
+  filtered_rpm = filtered_rpm + 0.1 * (new_rpm - filtered_rpm);
+  stsA.pause_long_us = (uint32_t)(60000000.0f / filtered_rpm / 2.0f);
   LOG_INFO("Spark Delay uS: ", stsA.spark_delay_us, "\tSoft Start: ", stsA.soft_start ? "TRUE" : "FALSE");
-  delay(500);
+  LOG_INFO("Engine Rpm: ", (uint32_t)(filtered_rpm));
+  LOG_INFO("Coil Pulse: ", stsA.coil_pulse_us, "us");
+  LOG_INFO("Spark Pulse: ", stsA.spark_pulse_us, "us");
+
+
+  delay(100);
+  clearScreen();
 }
diff --git a/RotaxMonitorTester/src/pins.h b/RotaxMonitorTester/src/pins.h
index a0b8c36..cced640 100644
--- a/RotaxMonitorTester/src/pins.h
+++ b/RotaxMonitorTester/src/pins.h
@@ -19,4 +19,5 @@
 #define SPARK_B34 5
 
 // Pot
-#define SPARK_DELAY_POT 12
+#define SPARK_DELAY_POT 13
+#define FREQ_POT 14