From 689f88ae846bd6c72a11396cf61647069b2f0c19 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 17:17:57 +0100
Subject: [PATCH 01/14] Initial code upload

---
 .../esp32/esp32_i2s_driver.cpp                | 350 ++++++++++++++++++
 1 file changed, 350 insertions(+)
 create mode 100644 src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
new file mode 100644
index 00000000..1d9e3ea7
--- /dev/null
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -0,0 +1,350 @@
+#include "../../hardware_api.h"
+#include "../../../drivers/hardware_api.h"
+#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC)
+
+#include <soc/sens_reg.h>
+#include <soc/sens_struct.h>
+
+#include <driver/i2s.h>
+#include <soc/i2s_reg.h>
+
+#include "soc/syscon_periph.h"
+#include <soc/syscon_struct.h>
+
+#include "soc/timer_group_struct.h"
+#include "soc/timer_group_reg.h"
+
+#include <soc/soc.h>
+#include <soc/dport_reg.h>
+
+#include <soc/adc_periph.h>
+#include <driver/adc.h>
+#include <driver/rtc_io.h>
+
+
+#define BUF_LEN 5
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 4095.0f
+
+#define I2S_USE_INTERRUPT false
+
+/**
+ *  I2S reading implementation by @mcells.
+ */
+
+uint32_t IRAM_ATTR adc_buffer[ADC1_CHANNEL_MAX] = {0};
+int globalActiveChannels = 0;
+int channels[ADC1_CHANNEL_MAX] = {0};
+bool running = false;
+
+#if DEBUG_ADC
+uint32_t IRAM_ATTR readscnt = 0;
+uint32_t IRAM_ATTR intcnt = 0;
+uint32_t IRAM_ATTR skipped = 0;
+uint32_t IRAM_ATTR equal = 0;
+uint32_t IRAM_ATTR currfifo = 0;
+uint32_t IRAM_ATTR lastfifo = 0;
+unsigned long IRAM_ATTR ts = 0;
+unsigned long IRAM_ATTR fifotime = 0;
+#endif
+
+// This function reads data from the I2S FIFO and processes it to obtain average readings for each channel.
+// The ADC counts get saved in uint32_t adc_buffer[].
+static void IRAM_ATTR readFiFo()
+{
+    // uint32_t readings[ADC1_CHANNEL_MAX][ADC1_CHANNEL_MAX*BUF_LEN];
+    uint32_t avgreadings[ADC1_CHANNEL_MAX] = {0};
+    uint32_t counts[ADC1_CHANNEL_MAX] = {0};
+    uint32_t fifolen = GET_PERI_REG_BITS2(I2S_FIFO_CONF_REG(0), I2S_RX_DATA_NUM_M, I2S_RX_DATA_NUM_S); // I2S0.fifo_conf.rx_data_num;
+
+#if DEBUG_ADC
+    uint32_t lastrd = 0;
+    uint32_t lasth = 0;
+    uint32_t lastl = 0;
+    uint32_t internalequal = 0;
+#endif
+
+    for (size_t i = 0; i < fifolen; i++)
+    {
+        // while(!GET_PERI_REG_MASK(I2S_INT_RAW_REG(0), I2S_RX_REMPTY_INT_RAW_M)){
+        // I2S0.in_fifo_pop.pop = 1;
+        // I2S0.in_fifo_pop.pop = 0;
+        SET_PERI_REG_MASK(I2S_INFIFO_POP_REG(0), I2S_INFIFO_POP_M);
+        CLEAR_PERI_REG_MASK(I2S_INFIFO_POP_REG(0), I2S_INFIFO_POP_M);
+
+        uint32_t rd = *(uint32_t *)(REG_I2S_BASE(0) + 0x4); // GET_PERI_REG_BITS2(I2S_FIFO_CONF_REG, I2S_RX_DATA_NUM_M, I2S_RX_DATA_NUM_S);I2S0.fifo_rd;
+
+        uint32_t highVal = rd >> 16;
+        uint32_t lowVal = rd & 0xFFFF;
+
+#if DEBUG_ADC
+    if (rd == lastrd || highVal == lastl || lowVal == lasth)
+    { //
+        internalequal++;
+        // currfifo = rd;
+        // lastfifo = lastrd;
+        // Serial.printf("\n|\nlast: ");
+        // for (int i = 31; i >= 15; i--)
+        // {
+        //   Serial.printf("%d",(lastfifo >> i ) & 1);
+        // }
+        // Serial.printf("  ");
+        // for (int i = 15; i >= 0; i--)
+        // {
+        //   Serial.printf("%d",(lastfifo >> i ) & 1);
+        // }
+
+        // Serial.printf("\ncurr: ");
+        // for (int i = 31; i >= 15; i--)
+        // {
+        //   Serial.printf("%d",(currfifo >> i ) & 1);
+        // }
+        // Serial.printf("  ");
+        // for (int i = 15; i >= 0; i--)
+        // {
+        //   Serial.printf("%d",(currfifo >> i ) & 1);
+        // }
+        // Serial.printf("\n");
+    }
+
+    lasth = highVal;
+    lastl = lowVal;
+    lastrd = rd;
+    readscnt += 2;
+#endif
+    uint32_t chan = (lowVal >> 12) & 0x07;
+    uint32_t adc_value = lowVal & 0xfff;
+    // readings[chan][counts[chan]] = adc_value;
+    avgreadings[chan] += adc_value;
+    counts[chan]++;
+
+    chan = (highVal >> 12) & 0x07;
+    adc_value = highVal & 0xfff;
+    // readings[chan][counts[chan]] = adc_value;
+    avgreadings[chan] += adc_value;
+    counts[chan]++;
+  }
+#if DEBUG_ADC
+  equal = internalequal;
+  intcnt += 1; // I2S0.fifo_conf.rx_data_num;
+#endif
+  for (int j = 0; j < ADC1_CHANNEL_MAX; j++)
+  {
+      if (counts[j] != 0)
+      {
+          adc_buffer[j] = avgreadings[j] / counts[j];
+
+          // int32_t leastdiff = 4095;
+          // uint32_t idx = 0;
+          // for (size_t k = 0; k < counts[j]; k++)
+          // {
+          //   int32_t diff = abs(int(adc_buffer[j]) - int(readings[j][k]));
+          //   if (leastdiff > diff && diff != 0)
+          //   {
+          //     leastdiff = diff;
+          //     idx = k;
+          //   }
+          // }
+          // adc_buffer[j] = readings[j][idx];
+
+          // Serial.printf(">Channel%d:%d\n", j, adc_buffer[j]);
+      }
+  }
+}
+
+
+static void IRAM_ATTR i2s_isr(void *arg)
+{
+#if DEBUG_ADC
+    unsigned long fifostart = micros();
+#endif
+
+    if (I2S0.int_st.rx_take_data) // fifo is full
+    {
+        readFiFo();
+    }
+
+    SET_PERI_REG_MASK(I2S_INT_CLR_REG(0), GET_PERI_REG_MASK(I2S_INT_ST_REG(0), 0xffff));
+    // I2S0.int_clr.val = I2S0.int_st.val;    // Clear all interrupt flags.
+
+#if DEBUG_ADC
+    fifotime = micros() - fifostart;
+#endif
+}
+
+// Contrary to its name (so it can be called by the library), this function reads the already converted values from fifo
+// and prints optional debug information.
+// When using interrupt driven sampling, it only prints debug information.
+void IRAM_ATTR _startADC3PinConversionLowSide()
+{
+#if I2S_USE_INTERRUPT != true
+    readFiFo();
+#endif
+
+#if DEBUG_ADC
+    skipped++;
+    if (skipped >= 1000)
+    {
+        skipped = 0;
+        unsigned long now = micros();
+        volatile uint32_t interr = intcnt;
+        volatile uint32_t samplesss = readscnt;
+        intcnt = 0;
+        readscnt = 0;
+
+        float readspersec = (1000000.0f * interr) / (now - ts);
+        float samplespersec = (1000000.0f * samplesss) / (now - ts);
+
+        ts = now;
+        Serial.printf(">ips:%f\n", readspersec);
+        Serial.printf(">sps:%f\n", samplespersec); // readspersec * GET_PERI_REG_BITS2(I2S_FIFO_CONF_REG(0), I2S_RX_DATA_NUM_M, I2S_RX_DATA_NUM_S)); //I2S0.fifo_conf.rx_data_num
+        Serial.printf(">fifo:%ld\n", fifotime);
+        Serial.printf(">doubles:%ld\n", equal);
+        // if(equal > 0){
+        //   volatile uint32_t aktuell = currfifo;
+        //   volatile uint32_t zuletzt = lastfifo;
+
+        // Serial.printf("\n|\nlast: ");
+        // for (int i = 31; i >= 15; i--)
+        // {
+        //   Serial.printf("%d",(zuletzt >> i ) & 1);
+        // }
+        // Serial.printf("  ");
+        // for (int i = 15; i >= 0; i--)
+        // {
+        //   Serial.printf("%d",(zuletzt >> i ) & 1);
+        // }
+
+        // Serial.printf("\ncurr: ");
+        // for (int i = 31; i >= 15; i--)
+        // {
+        //   Serial.printf("%d",(aktuell >> i ) & 1);
+        // }
+        // Serial.printf("  ");
+        // for (int i = 15; i >= 0; i--)
+        // {
+        //   Serial.printf("%d",(aktuell >> i ) & 1);
+        // }
+        // Serial.printf("\n");
+
+        // }
+
+        for (size_t i = 0; i < ADC1_CHANNEL_MAX; i++)
+        {
+            Serial.printf(">Channel%d:%d\n", i, adc_buffer[i]);
+        }
+    }
+#endif
+}
+
+// Takes the buffered adc counts and returns the coresponding float voltage for a pin.
+float IRAM_ATTR _readADCVoltageI2S(const int pin, const void *cs_params)
+{
+    float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams *)cs_params)->adc_voltage_conv;
+
+    return adc_buffer[digitalPinToAnalogChannel(pin)] * adc_voltage_conv;
+}
+
+// Sets up the I2S peripheral and ADC. Can be run multiple times to configure multiple motors.
+void* IRAM_ATTR _configureI2S(const void* driver_params, const int pinA, const int pinB, const int pinC){
+  mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit;
+
+  if( _isset(pinA) ) channels[digitalPinToAnalogChannel(pinA)] = 1; pinMode(pinA, GPIO_MODE_DISABLE );
+  if( _isset(pinB) ) channels[digitalPinToAnalogChannel(pinB)] = 1; pinMode(pinB, GPIO_MODE_DISABLE );
+  if( _isset(pinC) ) channels[digitalPinToAnalogChannel(pinC)] = 1; pinMode(pinC, GPIO_MODE_DISABLE );
+
+  int activeChannels = 0;
+  u_int some_channel = 0;
+  for (int i = 0; i < ADC1_CHANNEL_MAX; i++)
+  {
+      activeChannels += channels[i];
+      if (channels[i])
+      {
+          some_channel = channels[i];
+      }
+  }
+  globalActiveChannels = activeChannels;
+
+  ESP32MCPWMCurrentSenseParams *params = new ESP32MCPWMCurrentSenseParams{
+      .pins = {pinA, pinB, pinC},
+      .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION),
+  };
+
+  periph_module_reset(PERIPH_I2S0_MODULE);
+  periph_module_enable(PERIPH_I2S0_MODULE);
+
+  ESP_ERROR_CHECK(i2s_set_adc_mode(ADC_UNIT_1, (adc1_channel_t) some_channel));
+
+  // Taken from https://github.com/pycom/esp-idf-2.0/blob/master/components/bootloader_support/src/bootloader_random.c
+  // The set values are good I guess?
+  DPORT_SET_PERI_REG_MASK(DPORT_PERIP_CLK_EN_REG, DPORT_I2S0_CLK_EN);
+  CLEAR_PERI_REG_MASK(SENS_SAR_START_FORCE_REG, SENS_ULP_CP_FORCE_START_TOP);
+  CLEAR_PERI_REG_MASK(SENS_SAR_START_FORCE_REG, SENS_ULP_CP_START_TOP);
+
+  SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR, 3, SENS_FORCE_XPD_SAR_S);
+  SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DIG_FORCE);
+  CLEAR_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DIG_FORCE);
+  SET_PERI_REG_MASK(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_SAR2_MUX);
+  SET_PERI_REG_BITS(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_SAR_CLK_DIV, 4, SYSCON_SARADC_SAR_CLK_DIV_S);
+
+  SET_PERI_REG_BITS(SYSCON_SARADC_FSM_REG, SYSCON_SARADC_RSTB_WAIT, 8, SYSCON_SARADC_RSTB_WAIT_S);
+  SET_PERI_REG_BITS(SYSCON_SARADC_FSM_REG, SYSCON_SARADC_START_WAIT, 10, SYSCON_SARADC_START_WAIT_S);
+  SET_PERI_REG_BITS(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_WORK_MODE, 0, SYSCON_SARADC_WORK_MODE_S);
+  CLEAR_PERI_REG_MASK(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_SAR_SEL);
+  CLEAR_PERI_REG_MASK(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_DATA_SAR_SEL);
+
+  SET_PERI_REG_BITS(I2S_SAMPLE_RATE_CONF_REG(0), I2S_RX_BCK_DIV_NUM, 20, I2S_RX_BCK_DIV_NUM_S);
+
+  SET_PERI_REG_MASK(SYSCON_SARADC_CTRL_REG,SYSCON_SARADC_DATA_TO_I2S);
+  CLEAR_PERI_REG_MASK(I2S_CONF2_REG(0), I2S_CAMERA_EN);
+  SET_PERI_REG_MASK(I2S_CONF2_REG(0), I2S_LCD_EN);
+  SET_PERI_REG_MASK(I2S_CONF2_REG(0), I2S_DATA_ENABLE);
+  SET_PERI_REG_MASK(I2S_CONF_REG(0), I2S_RX_START);
+  // End
+
+  I2S0.conf_chan.rx_chan_mod = 2;
+
+  I2S0.fifo_conf.rx_data_num = max(8, min(63, (globalActiveChannels * BUF_LEN) / 2));
+  I2S0.fifo_conf.dscr_en = 0; // disable dma transfer.
+
+  I2S0.int_ena.val = 0;
+
+#if I2S_USE_INTERRUPT == true
+  I2S0.int_ena.rx_take_data = 1;
+// I2S0.int_ena.rx_rempty = 1;
+#endif
+
+    // ADC setting
+    int channelnum = 0;
+    for (size_t i = 0; i < ADC1_CHANNEL_MAX; i++)
+    {
+        if (channelnum <= 4 && channels[i] == 1)
+        {
+            SYSCON.saradc_sar1_patt_tab[0] |= ((i << 4) | (ADC_WIDTH_BIT_12 << 2) | ADC_ATTEN_DB_11) << (3 - channelnum) * 8;
+            channelnum++;
+        }
+        else if (channelnum > 4 && channels[i] == 1)
+        {
+            SYSCON.saradc_sar1_patt_tab[1] |= ((i << 4) | (ADC_WIDTH_BIT_12 << 2) | ADC_ATTEN_DB_11) << (3 - channelnum) * 8;
+            channelnum++;
+        }
+    }
+
+  // Scan multiple channels.
+  SET_PERI_REG_BITS(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_SAR1_PATT_LEN, channelnum - 1, SYSCON_SARADC_SAR1_PATT_LEN_S);
+
+  if (!running)
+  {
+#if I2S_USE_INTERRUPT == true
+      ESP_ERROR_CHECK(esp_intr_alloc(ETS_I2S0_INTR_SOURCE, ESP_INTR_FLAG_IRAM, i2s_isr, NULL, NULL));
+#endif
+
+      running = true;
+  }
+
+  return params;
+}
+
+#endif
\ No newline at end of file

From 9fcd4e5a3b2e7b48deeeb389e90278a08f88b0df Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 17:42:23 +0100
Subject: [PATCH 02/14] initial integration

---
 .../esp32/esp32_i2s_driver.cpp                | 40 +++++++++++--------
 .../hardware_specific/esp32/esp32_mcu.cpp     | 34 +++++++++++++---
 2 files changed, 52 insertions(+), 22 deletions(-)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index 1d9e3ea7..1acf5e94 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -34,11 +34,11 @@
  *  I2S reading implementation by @mcells.
  */
 
-uint32_t IRAM_ATTR adc_buffer[ADC1_CHANNEL_MAX] = {0};
+uint32_t IRAM_ATTR i2s_adc_buffer[ADC1_CHANNEL_MAX] = {0};
 int globalActiveChannels = 0;
 int channels[ADC1_CHANNEL_MAX] = {0};
 bool running = false;
-
+bool sampleOnCommand = true;
 #if DEBUG_ADC
 uint32_t IRAM_ATTR readscnt = 0;
 uint32_t IRAM_ATTR intcnt = 0;
@@ -51,7 +51,7 @@ unsigned long IRAM_ATTR fifotime = 0;
 #endif
 
 // This function reads data from the I2S FIFO and processes it to obtain average readings for each channel.
-// The ADC counts get saved in uint32_t adc_buffer[].
+// The ADC counts get saved in uint32_t i2s_adc_buffer[].
 static void IRAM_ATTR readFiFo()
 {
     // uint32_t readings[ADC1_CHANNEL_MAX][ADC1_CHANNEL_MAX*BUF_LEN];
@@ -134,22 +134,22 @@ static void IRAM_ATTR readFiFo()
   {
       if (counts[j] != 0)
       {
-          adc_buffer[j] = avgreadings[j] / counts[j];
+          i2s_adc_buffer[j] = avgreadings[j] / counts[j];
 
           // int32_t leastdiff = 4095;
           // uint32_t idx = 0;
           // for (size_t k = 0; k < counts[j]; k++)
           // {
-          //   int32_t diff = abs(int(adc_buffer[j]) - int(readings[j][k]));
+          //   int32_t diff = abs(int(i2s_adc_buffer[j]) - int(readings[j][k]));
           //   if (leastdiff > diff && diff != 0)
           //   {
           //     leastdiff = diff;
           //     idx = k;
           //   }
           // }
-          // adc_buffer[j] = readings[j][idx];
+          // i2s_adc_buffer[j] = readings[j][idx];
 
-          // Serial.printf(">Channel%d:%d\n", j, adc_buffer[j]);
+          // Serial.printf(">Channel%d:%d\n", j, i2s_adc_buffer[j]);
       }
   }
 }
@@ -180,7 +180,10 @@ static void IRAM_ATTR i2s_isr(void *arg)
 void IRAM_ATTR _startADC3PinConversionLowSide()
 {
 #if I2S_USE_INTERRUPT != true
-    readFiFo();
+    if (sampleOnCommand)
+    {
+        readFiFo();
+    }
 #endif
 
 #if DEBUG_ADC
@@ -233,7 +236,7 @@ void IRAM_ATTR _startADC3PinConversionLowSide()
 
         for (size_t i = 0; i < ADC1_CHANNEL_MAX; i++)
         {
-            Serial.printf(">Channel%d:%d\n", i, adc_buffer[i]);
+            Serial.printf(">Channel%d:%d\n", i, i2s_adc_buffer[i]);
         }
     }
 #endif
@@ -244,16 +247,18 @@ float IRAM_ATTR _readADCVoltageI2S(const int pin, const void *cs_params)
 {
     float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams *)cs_params)->adc_voltage_conv;
 
-    return adc_buffer[digitalPinToAnalogChannel(pin)] * adc_voltage_conv;
+    return i2s_adc_buffer[digitalPinToAnalogChannel(pin)] * adc_voltage_conv;
 }
 
 // Sets up the I2S peripheral and ADC. Can be run multiple times to configure multiple motors.
-void* IRAM_ATTR _configureI2S(const void* driver_params, const int pinA, const int pinB, const int pinC){
+void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, const int pinA, const int pinB, const int pinC){
+  sampleOnCommand = !lowside;
+
   mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit;
 
-  if( _isset(pinA) ) channels[digitalPinToAnalogChannel(pinA)] = 1; pinMode(pinA, GPIO_MODE_DISABLE );
-  if( _isset(pinB) ) channels[digitalPinToAnalogChannel(pinB)] = 1; pinMode(pinB, GPIO_MODE_DISABLE );
-  if( _isset(pinC) ) channels[digitalPinToAnalogChannel(pinC)] = 1; pinMode(pinC, GPIO_MODE_DISABLE );
+  if( _isset(pinA) ) channels[digitalPinToAnalogChannel(pinA)] = 1; pinMode(pinA, INPUT);
+  if( _isset(pinB) ) channels[digitalPinToAnalogChannel(pinB)] = 1; pinMode(pinB, INPUT);
+  if( _isset(pinC) ) channels[digitalPinToAnalogChannel(pinC)] = 1; pinMode(pinC, INPUT);
 
   int activeChannels = 0;
   u_int some_channel = 0;
@@ -267,9 +272,10 @@ void* IRAM_ATTR _configureI2S(const void* driver_params, const int pinA, const i
   }
   globalActiveChannels = activeChannels;
 
-  ESP32MCPWMCurrentSenseParams *params = new ESP32MCPWMCurrentSenseParams{
-      .pins = {pinA, pinB, pinC},
-      .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION),
+  ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams {
+    .pins = { pinA, pinB, pinC },
+    .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION),
+    .mcpwm_unit = unit
   };
 
   periph_module_reset(PERIPH_I2S0_MODULE);
diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
index 2057463c..bc081e2d 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
@@ -5,6 +5,7 @@
 #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC)
 
 #include "esp32_adc_driver.h"
+#include "esp32_i2s_driver.h"
 
 #include "driver/mcpwm.h"
 #include "soc/mcpwm_reg.h"
@@ -15,7 +16,7 @@
 
 #define _ADC_VOLTAGE 3.3f
 #define _ADC_RESOLUTION 4095.0f
-
+#define _I2S_ADC true
 
 typedef struct ESP32MCPWMCurrentSenseParams {
   int pins[3];
@@ -29,13 +30,21 @@ typedef struct ESP32MCPWMCurrentSenseParams {
  *  Inline adc reading implementation 
 */
 // function reading an ADC value and returning the read voltage
-float _readADCVoltageInline(const int pinA, const void* cs_params){
+float _readADCVoltageInline(const int pinA, const void *cs_params)
+{
+#if _I2S_ADC == true
+  return _readADCVoltageI2S(pinA, cs_params);
+#else
   uint32_t raw_adc = adcRead(pinA);
-  return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv;
+  return raw_adc * ((ESP32MCPWMCurrentSenseParams *)cs_params)->adc_voltage_conv;
+#endif
 }
 
 // function reading an ADC value and returning the read voltage
 void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){
+#if _I2S_ADC == true
+  return _configureI2S(false, driver_params, pinA, pinB, pinC);
+#else
   _UNUSED(driver_params);
 
   if( _isset(pinA) ) pinMode(pinA, INPUT);
@@ -46,8 +55,8 @@ void* _configureADCInline(const void* driver_params, const int pinA, const int p
     .pins = { pinA, pinB, pinC },
     .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
   };
-
   return params;
+#endif
 }
 
 
@@ -68,6 +77,9 @@ int adc_read_index[2]={0};
 
 // function reading an ADC value and returning the read voltage
 float _readADCVoltageLowSide(const int pin, const void* cs_params){
+#if _I2S_ADC == true
+  return _readADCVoltageI2S(pin, cs_params);
+#else
   mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit;
   int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index;
   float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv;
@@ -78,11 +90,14 @@ float _readADCVoltageLowSide(const int pin, const void* cs_params){
   }
   // not found
   return  0;
+#endif
 }
 
 // function configuring low-side current sensing 
 void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){
-  
+#if _I2S_ADC == true
+  return _configureI2S(true, driver_params, pinA, pinB, pinC);
+#else
   mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit;
   int index_start = adc_pin_count[unit];
   if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA;
@@ -101,6 +116,7 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p
   };
 
   return params;
+#endif
 }
 
 
@@ -131,9 +147,13 @@ static void IRAM_ATTR mcpwm0_isr_handler(void*){
   // low side
   uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st;
   if(mcpwm_intr_status){
+#if _I2S_ADC == true
+      readFiFo();
+#else
     adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]);
     adc_read_index[0]++;
     if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0;
+#endif
   }
   // low side
   MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status;
@@ -151,9 +171,13 @@ static void IRAM_ATTR mcpwm1_isr_handler(void*){
   // low side
   uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st;
   if(mcpwm_intr_status){
+#if _I2S_ADC == true
+    readFiFo();
+#else
     adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]);
     adc_read_index[1]++;
     if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0;
+#endif
   }
   // low side
   MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status;

From b68fedfe709c683aa3c3e464e01d1bd879f38d39 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 17:49:17 +0100
Subject: [PATCH 03/14] Add conversion command for inline

---
 src/current_sense/InlineCurrentSense.cpp                      | 2 ++
 .../hardware_specific/esp32/esp32_i2s_driver.cpp              | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp
index c3db74ef..17793d53 100644
--- a/src/current_sense/InlineCurrentSense.cpp
+++ b/src/current_sense/InlineCurrentSense.cpp
@@ -61,6 +61,7 @@ void InlineCurrentSense::calibrateOffsets(){
     offset_ic = 0;
     // read the adc voltage 1000 times ( arbitrary number )
     for (int i = 0; i < calibration_rounds; i++) {
+        _startADC3PinConversionInline();
         if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params);
         if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params);
         if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params);
@@ -75,6 +76,7 @@ void InlineCurrentSense::calibrateOffsets(){
 // read all three phase currents (if possible 2 or 3)
 PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
     PhaseCurrent_s current;
+    _startADC3PinConversionInline();
     current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps
     current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps
     current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index 1acf5e94..deeb61b9 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -242,6 +242,10 @@ void IRAM_ATTR _startADC3PinConversionLowSide()
 #endif
 }
 
+void IRAM_ATTR _startADC3PinConversionInline(){
+    _startADC3PinConversionLowSide();
+}
+
 // Takes the buffered adc counts and returns the coresponding float voltage for a pin.
 float IRAM_ATTR _readADCVoltageI2S(const int pin, const void *cs_params)
 {

From cbe9fd0767857949875ac71d7b99536caf81318f Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 17:57:29 +0100
Subject: [PATCH 04/14] add header file

---
 src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index deeb61b9..d497277b 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -2,6 +2,8 @@
 #include "../../../drivers/hardware_api.h"
 #include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h"
 
+#include "esp32_i2s_driver.h"
+
 #if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC)
 
 #include <soc/sens_reg.h>

From cee79a9afaa2152be922b004a7170b58bd1d9b48 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 17:58:13 +0100
Subject: [PATCH 05/14] add header file for real

---
 .../esp32/esp32_i2s_driver.h                  | 27 +++++++++++++++++++
 1 file changed, 27 insertions(+)
 create mode 100644 src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
new file mode 100644
index 00000000..0783902b
--- /dev/null
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
@@ -0,0 +1,27 @@
+#ifndef SIMPLEFOC_ESP32_HAL_I2S_DRIVER_H_ 
+#define SIMPLEFOC_ESP32_HAL_I2S_DRIVER_H_
+
+#include "Arduino.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) 
+
+// This function reads data from the I2S FIFO and processes it to obtain average readings for each channel.
+// The ADC counts get saved in uint32_t i2s_adc_buffer[].
+static void IRAM_ATTR readFiFo();
+
+static void IRAM_ATTR i2s_isr(void *arg);
+
+// Contrary to its name (so it can be called by the library), this function reads the already converted values from fifo
+// and prints optional debug information.
+// When using interrupt driven sampling, it only prints debug information.
+void IRAM_ATTR _startADC3PinConversionLowSide();
+void IRAM_ATTR _startADC3PinConversionInline();
+
+// Takes the buffered adc counts and returns the coresponding float voltage for a pin.
+float IRAM_ATTR _readADCVoltageI2S(const int pin, const void *cs_params);
+
+// Sets up the I2S peripheral and ADC. Can be run multiple times to configure multiple motors.
+void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, const int pinA, const int pinB, const int pinC);
+
+#endif /* SIMPLEFOC_ESP32_HAL_I2S_DRIVER_H_ */
+#endif /* ESP32 */
\ No newline at end of file

From 6e9dfe0851d46606114c994c9febeb2ed381d189 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 18:00:29 +0100
Subject: [PATCH 06/14] add function def

---
 src/current_sense/hardware_api.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h
index 7862b708..9765da22 100644
--- a/src/current_sense/hardware_api.h
+++ b/src/current_sense/hardware_api.h
@@ -35,6 +35,8 @@ float _readADCVoltageInline(const int pinA, const void* cs_params);
  */
 void* _configureADCInline(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET);
 
+void _startADC3PinConversionInline();
+
 /**
  *  function reading an ADC value and returning the read voltage
  *

From 669c0cdefd9aa676b1fb5b2709291510b1b184f0 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 18:06:09 +0100
Subject: [PATCH 07/14] remove statics

---
 .../hardware_specific/esp32/esp32_i2s_driver.cpp     |  2 +-
 .../hardware_specific/esp32/esp32_i2s_driver.h       | 12 +++++-------
 2 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index d497277b..b1429a5d 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -54,7 +54,7 @@ unsigned long IRAM_ATTR fifotime = 0;
 
 // This function reads data from the I2S FIFO and processes it to obtain average readings for each channel.
 // The ADC counts get saved in uint32_t i2s_adc_buffer[].
-static void IRAM_ATTR readFiFo()
+void IRAM_ATTR readFiFo()
 {
     // uint32_t readings[ADC1_CHANNEL_MAX][ADC1_CHANNEL_MAX*BUF_LEN];
     uint32_t avgreadings[ADC1_CHANNEL_MAX] = {0};
diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
index 0783902b..9d5c79cf 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
@@ -7,21 +7,19 @@
 
 // This function reads data from the I2S FIFO and processes it to obtain average readings for each channel.
 // The ADC counts get saved in uint32_t i2s_adc_buffer[].
-static void IRAM_ATTR readFiFo();
-
-static void IRAM_ATTR i2s_isr(void *arg);
+void readFiFo();
 
 // Contrary to its name (so it can be called by the library), this function reads the already converted values from fifo
 // and prints optional debug information.
 // When using interrupt driven sampling, it only prints debug information.
-void IRAM_ATTR _startADC3PinConversionLowSide();
-void IRAM_ATTR _startADC3PinConversionInline();
+void _startADC3PinConversionLowSide();
+void _startADC3PinConversionInline();
 
 // Takes the buffered adc counts and returns the coresponding float voltage for a pin.
-float IRAM_ATTR _readADCVoltageI2S(const int pin, const void *cs_params);
+float _readADCVoltageI2S(const int pin, const void *cs_params);
 
 // Sets up the I2S peripheral and ADC. Can be run multiple times to configure multiple motors.
-void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, const int pinA, const int pinB, const int pinC);
+void* _configureI2S(const bool lowside, const void* driver_params, const int pinA, const int pinB, const int pinC);
 
 #endif /* SIMPLEFOC_ESP32_HAL_I2S_DRIVER_H_ */
 #endif /* ESP32 */
\ No newline at end of file

From e5205b7bd1e6d422a06286dd9c6029bebbdb5cac Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 18:11:54 +0100
Subject: [PATCH 08/14] compile out isr to make unit tests happy

---
 .../hardware_specific/esp32/esp32_i2s_driver.cpp              | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index b1429a5d..2b15d8fd 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -156,7 +156,7 @@ void IRAM_ATTR readFiFo()
   }
 }
 
-
+#if I2S_USE_INTERRUPT == true
 static void IRAM_ATTR i2s_isr(void *arg)
 {
 #if DEBUG_ADC
@@ -175,7 +175,7 @@ static void IRAM_ATTR i2s_isr(void *arg)
     fifotime = micros() - fifostart;
 #endif
 }
-
+#endif
 // Contrary to its name (so it can be called by the library), this function reads the already converted values from fifo
 // and prints optional debug information.
 // When using interrupt driven sampling, it only prints debug information.

From 36d075216430b2eede2685bb658fe81d15ecaf70 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 18:23:52 +0100
Subject: [PATCH 09/14] moar include

---
 src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index 2b15d8fd..ad2eadcd 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -14,6 +14,7 @@
 
 #include "soc/syscon_periph.h"
 #include <soc/syscon_struct.h>
+#include <soc/syscon_reg.h>
 
 #include "soc/timer_group_struct.h"
 #include "soc/timer_group_reg.h"

From db946122b7e0faac8edd5020f732056195d109d7 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 18:39:23 +0100
Subject: [PATCH 10/14] i hate errors

---
 .../hardware_specific/esp32/esp32_i2s_driver.cpp          | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index ad2eadcd..a768cb27 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -33,6 +33,14 @@
 
 #define I2S_USE_INTERRUPT false
 
+typedef struct ESP32MCPWMCurrentSenseParams {
+  int pins[3];
+  float adc_voltage_conv;
+  mcpwm_unit_t mcpwm_unit;
+  int buffer_index;
+} ESP32MCPWMCurrentSenseParams;
+
+
 /**
  *  I2S reading implementation by @mcells.
  */

From 4acbcd441ad934b7cb819548997bf192060639a1 Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Mon, 12 Feb 2024 19:34:51 +0100
Subject: [PATCH 11/14] fix init

---
 src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index a768cb27..3440b49c 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -296,7 +296,8 @@ void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, con
   periph_module_reset(PERIPH_I2S0_MODULE);
   periph_module_enable(PERIPH_I2S0_MODULE);
 
-  ESP_ERROR_CHECK(i2s_set_adc_mode(ADC_UNIT_1, (adc1_channel_t) some_channel));
+  // ESP_ERROR_CHECK(i2s_set_adc_mode(ADC_UNIT_1, (adc1_channel_t) some_channel));
+  ESP_ERROR_CHECK(i2s_set_adc_mode(ADC_UNIT_1, ADC1_CHANNEL_0));
 
   // Taken from https://github.com/pycom/esp-idf-2.0/blob/master/components/bootloader_support/src/bootloader_random.c
   // The set values are good I guess?

From e390a3279ed04cb0139942fe1b2de185ea957afa Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Sat, 17 Feb 2024 20:02:28 +0100
Subject: [PATCH 12/14] Prevent pinMode related crashes in loopfoc ISR &
 Possibly cleaner sampling strategy

---
 .../esp32/esp32_i2s_driver.cpp                | 27 ++++++++++---------
 .../hardware_specific/esp32/esp32_mcu.cpp     | 18 +++++++------
 2 files changed, 25 insertions(+), 20 deletions(-)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index 3440b49c..924efb84 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -14,7 +14,7 @@
 
 #include "soc/syscon_periph.h"
 #include <soc/syscon_struct.h>
-#include <soc/syscon_reg.h>
+// #include <soc/syscon_reg.h>
 
 #include "soc/timer_group_struct.h"
 #include "soc/timer_group_reg.h"
@@ -27,11 +27,12 @@
 #include <driver/rtc_io.h>
 
 
-#define BUF_LEN 5
+#define BUF_LEN 1
 #define _ADC_VOLTAGE 3.3f
 #define _ADC_RESOLUTION 4095.0f
 
 #define I2S_USE_INTERRUPT false
+#define DEBUG_ADC false
 
 typedef struct ESP32MCPWMCurrentSenseParams {
   int pins[3];
@@ -50,7 +51,7 @@ int globalActiveChannels = 0;
 int channels[ADC1_CHANNEL_MAX] = {0};
 bool running = false;
 bool sampleOnCommand = true;
-#if DEBUG_ADC
+#if DEBUG_ADC == true
 uint32_t IRAM_ATTR readscnt = 0;
 uint32_t IRAM_ATTR intcnt = 0;
 uint32_t IRAM_ATTR skipped = 0;
@@ -90,7 +91,7 @@ void IRAM_ATTR readFiFo()
         uint32_t highVal = rd >> 16;
         uint32_t lowVal = rd & 0xFFFF;
 
-#if DEBUG_ADC
+#if DEBUG_ADC == true
     if (rd == lastrd || highVal == lastl || lowVal == lasth)
     { //
         internalequal++;
@@ -137,7 +138,7 @@ void IRAM_ATTR readFiFo()
     avgreadings[chan] += adc_value;
     counts[chan]++;
   }
-#if DEBUG_ADC
+#if DEBUG_ADC == true
   equal = internalequal;
   intcnt += 1; // I2S0.fifo_conf.rx_data_num;
 #endif
@@ -168,7 +169,7 @@ void IRAM_ATTR readFiFo()
 #if I2S_USE_INTERRUPT == true
 static void IRAM_ATTR i2s_isr(void *arg)
 {
-#if DEBUG_ADC
+#if DEBUG_ADC == true
     unsigned long fifostart = micros();
 #endif
 
@@ -180,11 +181,12 @@ static void IRAM_ATTR i2s_isr(void *arg)
     SET_PERI_REG_MASK(I2S_INT_CLR_REG(0), GET_PERI_REG_MASK(I2S_INT_ST_REG(0), 0xffff));
     // I2S0.int_clr.val = I2S0.int_st.val;    // Clear all interrupt flags.
 
-#if DEBUG_ADC
+#if DEBUG_ADC == true
     fifotime = micros() - fifostart;
 #endif
 }
 #endif
+
 // Contrary to its name (so it can be called by the library), this function reads the already converted values from fifo
 // and prints optional debug information.
 // When using interrupt driven sampling, it only prints debug information.
@@ -197,7 +199,7 @@ void IRAM_ATTR _startADC3PinConversionLowSide()
     }
 #endif
 
-#if DEBUG_ADC
+#if DEBUG_ADC == true
     skipped++;
     if (skipped >= 1000)
     {
@@ -271,9 +273,9 @@ void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, con
 
   mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit;
 
-  if( _isset(pinA) ) channels[digitalPinToAnalogChannel(pinA)] = 1; pinMode(pinA, INPUT);
-  if( _isset(pinB) ) channels[digitalPinToAnalogChannel(pinB)] = 1; pinMode(pinB, INPUT);
-  if( _isset(pinC) ) channels[digitalPinToAnalogChannel(pinC)] = 1; pinMode(pinC, INPUT);
+  if( _isset(pinA) ) channels[digitalPinToAnalogChannel(pinA)] = 1; // pinMode(pinA, GPIO_MODE_DISABLE);
+  if( _isset(pinB) ) channels[digitalPinToAnalogChannel(pinB)] = 1; // pinMode(pinB, GPIO_MODE_DISABLE);
+  if( _isset(pinC) ) channels[digitalPinToAnalogChannel(pinC)] = 1; // pinMode(pinC, GPIO_MODE_DISABLE);
 
   int activeChannels = 0;
   u_int some_channel = 0;
@@ -310,6 +312,7 @@ void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, con
   CLEAR_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DIG_FORCE);
   SET_PERI_REG_MASK(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_SAR2_MUX);
   SET_PERI_REG_BITS(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_SAR_CLK_DIV, 4, SYSCON_SARADC_SAR_CLK_DIV_S);
+  SET_PERI_REG_BITS(SYSCON_SARADC_CTRL_REG, SYSCON_SARADC_SAMPLE_CYCLE, 16, SYSCON_SARADC_SAMPLE_CYCLE_S);
 
   SET_PERI_REG_BITS(SYSCON_SARADC_FSM_REG, SYSCON_SARADC_RSTB_WAIT, 8, SYSCON_SARADC_RSTB_WAIT_S);
   SET_PERI_REG_BITS(SYSCON_SARADC_FSM_REG, SYSCON_SARADC_START_WAIT, 10, SYSCON_SARADC_START_WAIT_S);
@@ -328,7 +331,7 @@ void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, con
 
   I2S0.conf_chan.rx_chan_mod = 2;
 
-  I2S0.fifo_conf.rx_data_num = max(8, min(63, (globalActiveChannels * BUF_LEN) / 2));
+  I2S0.fifo_conf.rx_data_num = max(2, min(63, (globalActiveChannels * BUF_LEN) / 2));
   I2S0.fifo_conf.dscr_en = 0; // disable dma transfer.
 
   I2S0.int_ena.val = 0;
diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
index bc081e2d..246524e3 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
@@ -132,9 +132,9 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){
 
   // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL)
   if(mcpwm_unit == MCPWM_UNIT_0)
-    mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL);  //Set ISR Handler
+    mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM | ESP_INTR_FLAG_LEVEL1, NULL);  //Set ISR Handler
   else
-    mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL);  //Set ISR Handler
+    mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM | ESP_INTR_FLAG_LEVEL1, NULL);  //Set ISR Handler
 }
 
 static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused));
@@ -143,18 +143,19 @@ static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused));
 static void IRAM_ATTR mcpwm0_isr_handler(void*){
   // // high side
   // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st;
-  
+
   // low side
   uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st;
   if(mcpwm_intr_status){
 #if _I2S_ADC == true
+      delayMicroseconds(2);
       readFiFo();
 #else
-    adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]);
-    adc_read_index[0]++;
-    if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0;
+      adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]);
+      adc_read_index[0]++;
+      if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0;
 #endif
-  }
+      }
   // low side
   MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status;
   // high side
@@ -167,11 +168,12 @@ static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused));
 static void IRAM_ATTR mcpwm1_isr_handler(void*){
   // // high side
   // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st;
-  
+
   // low side
   uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st;
   if(mcpwm_intr_status){
 #if _I2S_ADC == true
+    delayMicroseconds(2);
     readFiFo();
 #else
     adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]);

From 3f3aba802ee7bc6031b55bbd1b86fe1a9016c09b Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Fri, 23 Feb 2024 20:56:36 +0100
Subject: [PATCH 13/14] Take only one Sample (good for HFI), read whole fifo
 and move debug to function

---
 .../esp32/esp32_i2s_driver.cpp                | 235 +++++++++---------
 .../esp32/esp32_i2s_driver.h                  |   2 +
 .../hardware_specific/esp32/esp32_mcu.cpp     |  74 +++---
 3 files changed, 168 insertions(+), 143 deletions(-)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index 924efb84..9a638024 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -46,13 +46,14 @@ typedef struct ESP32MCPWMCurrentSenseParams {
  *  I2S reading implementation by @mcells.
  */
 
-uint32_t IRAM_ATTR i2s_adc_buffer[ADC1_CHANNEL_MAX] = {0};
+static uint32_t IRAM_ATTR i2s_adc_buffer[ADC1_CHANNEL_MAX] = {0};
 int globalActiveChannels = 0;
 int channels[ADC1_CHANNEL_MAX] = {0};
 bool running = false;
 bool sampleOnCommand = true;
 #if DEBUG_ADC == true
 uint32_t IRAM_ATTR readscnt = 0;
+volatile uint32_t IRAM_ATTR buffersread = 0;
 uint32_t IRAM_ATTR intcnt = 0;
 uint32_t IRAM_ATTR skipped = 0;
 uint32_t IRAM_ATTR equal = 0;
@@ -70,19 +71,19 @@ void IRAM_ATTR readFiFo()
     uint32_t avgreadings[ADC1_CHANNEL_MAX] = {0};
     uint32_t counts[ADC1_CHANNEL_MAX] = {0};
     uint32_t fifolen = GET_PERI_REG_BITS2(I2S_FIFO_CONF_REG(0), I2S_RX_DATA_NUM_M, I2S_RX_DATA_NUM_S); // I2S0.fifo_conf.rx_data_num;
-
 #if DEBUG_ADC
     uint32_t lastrd = 0;
     uint32_t lasth = 0;
     uint32_t lastl = 0;
     uint32_t internalequal = 0;
+    uint32_t internal_buffersread = 0;
 #endif
 
-    for (size_t i = 0; i < fifolen; i++)
-    {
-        // while(!GET_PERI_REG_MASK(I2S_INT_RAW_REG(0), I2S_RX_REMPTY_INT_RAW_M)){
-        // I2S0.in_fifo_pop.pop = 1;
-        // I2S0.in_fifo_pop.pop = 0;
+    I2S0.int_ena.rx_rempty = 1;
+    I2S0.int_clr.rx_rempty = 1;
+
+    // FiFo length seems a fixed size -> read until empty for latest samples
+    while(!GET_PERI_REG_MASK(I2S_INT_RAW_REG(0), I2S_RX_REMPTY_INT_RAW_M)){
         SET_PERI_REG_MASK(I2S_INFIFO_POP_REG(0), I2S_INFIFO_POP_M);
         CLEAR_PERI_REG_MASK(I2S_INFIFO_POP_REG(0), I2S_INFIFO_POP_M);
 
@@ -92,57 +93,65 @@ void IRAM_ATTR readFiFo()
         uint32_t lowVal = rd & 0xFFFF;
 
 #if DEBUG_ADC == true
-    if (rd == lastrd || highVal == lastl || lowVal == lasth)
-    { //
-        internalequal++;
-        // currfifo = rd;
-        // lastfifo = lastrd;
-        // Serial.printf("\n|\nlast: ");
-        // for (int i = 31; i >= 15; i--)
-        // {
-        //   Serial.printf("%d",(lastfifo >> i ) & 1);
-        // }
-        // Serial.printf("  ");
-        // for (int i = 15; i >= 0; i--)
-        // {
-        //   Serial.printf("%d",(lastfifo >> i ) & 1);
-        // }
-
-        // Serial.printf("\ncurr: ");
-        // for (int i = 31; i >= 15; i--)
-        // {
-        //   Serial.printf("%d",(currfifo >> i ) & 1);
-        // }
-        // Serial.printf("  ");
-        // for (int i = 15; i >= 0; i--)
-        // {
-        //   Serial.printf("%d",(currfifo >> i ) & 1);
-        // }
-        // Serial.printf("\n");
-    }
+        if (rd == lastrd || highVal == lastl || lowVal == lasth)
+        { //
+            internalequal++;
+            // currfifo = rd;
+            // lastfifo = lastrd;
+            // Serial.printf("\n|\nlast: ");
+            // for (int i = 31; i >= 15; i--)
+            // {
+            //   Serial.printf("%d",(lastfifo >> i ) & 1);
+            // }
+            // Serial.printf("  ");
+            // for (int i = 15; i >= 0; i--)
+            // {
+            //   Serial.printf("%d",(lastfifo >> i ) & 1);
+            // }
+
+            // Serial.printf("\ncurr: ");
+            // for (int i = 31; i >= 15; i--)
+            // {
+            //   Serial.printf("%d",(currfifo >> i ) & 1);
+            // }
+            // Serial.printf("  ");
+            // for (int i = 15; i >= 0; i--)
+            // {
+            //   Serial.printf("%d",(currfifo >> i ) & 1);
+            // }
+            // Serial.printf("\n");
+        }
 
-    lasth = highVal;
-    lastl = lowVal;
-    lastrd = rd;
-    readscnt += 2;
+        lasth = highVal;
+        lastl = lowVal;
+        lastrd = rd;
+        readscnt += 2;
+        internal_buffersread++;
 #endif
-    uint32_t chan = (lowVal >> 12) & 0x07;
-    uint32_t adc_value = lowVal & 0xfff;
-    // readings[chan][counts[chan]] = adc_value;
-    avgreadings[chan] += adc_value;
-    counts[chan]++;
-
-    chan = (highVal >> 12) & 0x07;
-    adc_value = highVal & 0xfff;
-    // readings[chan][counts[chan]] = adc_value;
-    avgreadings[chan] += adc_value;
-    counts[chan]++;
-  }
+        uint32_t chan = (lowVal >> 12) & 0x07;
+        uint32_t adc_value = lowVal & 0xfff;
+        // readings[chan][counts[chan]] = adc_value;
+        avgreadings[chan] = adc_value;
+        i2s_adc_buffer[chan] = adc_value;
+        //avgreadings[chan] += adc_value;
+        counts[chan]++;
+
+        chan = (highVal >> 12) & 0x07;
+        adc_value = highVal & 0xfff;
+        // readings[chan][counts[chan]] = adc_value;
+        avgreadings[chan] = adc_value;
+        i2s_adc_buffer[chan] = adc_value;
+        //avgreadings[chan] += adc_value;
+        counts[chan]++;
+    }
+    I2S0.int_ena.rx_rempty = 0;
+    I2S0.int_clr.rx_rempty = 1;
 #if DEBUG_ADC == true
   equal = internalequal;
   intcnt += 1; // I2S0.fifo_conf.rx_data_num;
+  buffersread = internal_buffersread;
 #endif
-  for (int j = 0; j < ADC1_CHANNEL_MAX; j++)
+/*for (int j = 0; j < ADC1_CHANNEL_MAX; j++)
   {
       if (counts[j] != 0)
       {
@@ -163,7 +172,7 @@ void IRAM_ATTR readFiFo()
 
           // Serial.printf(">Channel%d:%d\n", j, i2s_adc_buffer[j]);
       }
-  }
+  }*/
 }
 
 #if I2S_USE_INTERRUPT == true
@@ -173,7 +182,7 @@ static void IRAM_ATTR i2s_isr(void *arg)
     unsigned long fifostart = micros();
 #endif
 
-    if (I2S0.int_st.rx_take_data) // fifo is full
+    if (I2S0.int_st.rx_wfull) // fifo is full
     {
         readFiFo();
     }
@@ -187,6 +196,64 @@ static void IRAM_ATTR i2s_isr(void *arg)
 }
 #endif
 
+void printdbg(){
+    #if DEBUG_ADC == true
+        // skipped++;
+        // if (skipped >= 1000)
+        {
+            // skipped = 0;
+            unsigned long now = micros();
+            volatile uint32_t interr = intcnt;
+            volatile uint32_t samplesss = readscnt;
+            intcnt = 0;
+            readscnt = 0;
+
+            float readspersec = (1000000.0f * interr) / (now - ts);
+            float samplespersec = (1000000.0f * samplesss) / (now - ts);
+
+            ts = now;
+            Serial.printf(">ips:%f\n", readspersec);
+            Serial.printf(">sps:%f\n", samplespersec); // readspersec * GET_PERI_REG_BITS2(I2S_FIFO_CONF_REG(0), I2S_RX_DATA_NUM_M, I2S_RX_DATA_NUM_S)); //I2S0.fifo_conf.rx_data_num
+            Serial.printf(">fifo:%ld\n", fifotime);
+            Serial.printf(">doubles:%ld\n", equal);
+            Serial.printf(">bufreads:%d\n", buffersread);
+            // if(equal > 0){
+            //   volatile uint32_t aktuell = currfifo;
+            //   volatile uint32_t zuletzt = lastfifo;
+
+            // Serial.printf("\n|\nlast: ");
+            // for (int i = 31; i >= 15; i--)
+            // {
+            //   Serial.printf("%d",(zuletzt >> i ) & 1);
+            // }
+            // Serial.printf("  ");
+            // for (int i = 15; i >= 0; i--)
+            // {
+            //   Serial.printf("%d",(zuletzt >> i ) & 1);
+            // }
+
+            // Serial.printf("\ncurr: ");
+            // for (int i = 31; i >= 15; i--)
+            // {
+            //   Serial.printf("%d",(aktuell >> i ) & 1);
+            // }
+            // Serial.printf("  ");
+            // for (int i = 15; i >= 0; i--)
+            // {
+            //   Serial.printf("%d",(aktuell >> i ) & 1);
+            // }
+            // Serial.printf("\n");
+
+            // }
+
+            for (size_t i = 0; i < ADC1_CHANNEL_MAX; i++)
+            {
+                Serial.printf(">Channel%d:%d\n", i, i2s_adc_buffer[i]);
+            }
+        }
+    #endif
+}
+
 // Contrary to its name (so it can be called by the library), this function reads the already converted values from fifo
 // and prints optional debug information.
 // When using interrupt driven sampling, it only prints debug information.
@@ -198,61 +265,6 @@ void IRAM_ATTR _startADC3PinConversionLowSide()
         readFiFo();
     }
 #endif
-
-#if DEBUG_ADC == true
-    skipped++;
-    if (skipped >= 1000)
-    {
-        skipped = 0;
-        unsigned long now = micros();
-        volatile uint32_t interr = intcnt;
-        volatile uint32_t samplesss = readscnt;
-        intcnt = 0;
-        readscnt = 0;
-
-        float readspersec = (1000000.0f * interr) / (now - ts);
-        float samplespersec = (1000000.0f * samplesss) / (now - ts);
-
-        ts = now;
-        Serial.printf(">ips:%f\n", readspersec);
-        Serial.printf(">sps:%f\n", samplespersec); // readspersec * GET_PERI_REG_BITS2(I2S_FIFO_CONF_REG(0), I2S_RX_DATA_NUM_M, I2S_RX_DATA_NUM_S)); //I2S0.fifo_conf.rx_data_num
-        Serial.printf(">fifo:%ld\n", fifotime);
-        Serial.printf(">doubles:%ld\n", equal);
-        // if(equal > 0){
-        //   volatile uint32_t aktuell = currfifo;
-        //   volatile uint32_t zuletzt = lastfifo;
-
-        // Serial.printf("\n|\nlast: ");
-        // for (int i = 31; i >= 15; i--)
-        // {
-        //   Serial.printf("%d",(zuletzt >> i ) & 1);
-        // }
-        // Serial.printf("  ");
-        // for (int i = 15; i >= 0; i--)
-        // {
-        //   Serial.printf("%d",(zuletzt >> i ) & 1);
-        // }
-
-        // Serial.printf("\ncurr: ");
-        // for (int i = 31; i >= 15; i--)
-        // {
-        //   Serial.printf("%d",(aktuell >> i ) & 1);
-        // }
-        // Serial.printf("  ");
-        // for (int i = 15; i >= 0; i--)
-        // {
-        //   Serial.printf("%d",(aktuell >> i ) & 1);
-        // }
-        // Serial.printf("\n");
-
-        // }
-
-        for (size_t i = 0; i < ADC1_CHANNEL_MAX; i++)
-        {
-            Serial.printf(">Channel%d:%d\n", i, i2s_adc_buffer[i]);
-        }
-    }
-#endif
 }
 
 void IRAM_ATTR _startADC3PinConversionInline(){
@@ -335,9 +347,10 @@ void* IRAM_ATTR _configureI2S(const bool lowside, const void* driver_params, con
   I2S0.fifo_conf.dscr_en = 0; // disable dma transfer.
 
   I2S0.int_ena.val = 0;
-
 #if I2S_USE_INTERRUPT == true
-  I2S0.int_ena.rx_take_data = 1;
+  I2S0.int_ena.rx_wfull = 1;
+
+//   I2S0.int_ena.rx_take_data = 1;
 // I2S0.int_ena.rx_rempty = 1;
 #endif
 
diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
index 9d5c79cf..b04fc44e 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.h
@@ -9,6 +9,8 @@
 // The ADC counts get saved in uint32_t i2s_adc_buffer[].
 void readFiFo();
 
+void printdbg();
+
 // Contrary to its name (so it can be called by the library), this function reads the already converted values from fifo
 // and prints optional debug information.
 // When using interrupt driven sampling, it only prints debug information.
diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
index 246524e3..17caeaf8 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
@@ -17,6 +17,7 @@
 #define _ADC_VOLTAGE 3.3f
 #define _ADC_RESOLUTION 4095.0f
 #define _I2S_ADC true
+#define SAMPLE_BOTH false
 
 typedef struct ESP32MCPWMCurrentSenseParams {
   int pins[3];
@@ -76,7 +77,7 @@ uint32_t adc_buffer[2][6]={0};
 int adc_read_index[2]={0};
 
 // function reading an ADC value and returning the read voltage
-float _readADCVoltageLowSide(const int pin, const void* cs_params){
+float IRAM_ATTR _readADCVoltageLowSide(const int pin, const void* cs_params){
 #if _I2S_ADC == true
   return _readADCVoltageI2S(pin, cs_params);
 #else
@@ -124,12 +125,16 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){
 
   mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev;
   mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit;
-
-  // low-side register enable interrupt
-  mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
-  // high side registers enable interrupt 
-  //mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt 
-
+  #if SAMPLE_BOTH == true
+    // low-side & high side register enable interrupt
+    mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
+    mcpwm_dev->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt 
+  #else
+    // low-side register enable interrupt
+    mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
+    // high side registers enable interrupt 
+    // mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt 
+  #endif
   // register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL)
   if(mcpwm_unit == MCPWM_UNIT_0)
     mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM | ESP_INTR_FLAG_LEVEL1, NULL);  //Set ISR Handler
@@ -141,50 +146,55 @@ static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused));
 
 // Read currents when interrupt is triggered
 static void IRAM_ATTR mcpwm0_isr_handler(void*){
-  // // high side
-  // uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st;
+  // high side
+  uint32_t mcpwm_intr_status_high = MCPWM0.int_st.timer0_tez_int_st;
 
   // low side
-  uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st;
-  if(mcpwm_intr_status){
-#if _I2S_ADC == true
-      delayMicroseconds(2);
+  uint32_t mcpwm_intr_status_low = MCPWM0.int_st.timer0_tep_int_st;
+
+  bool runadc = mcpwm_intr_status_high || mcpwm_intr_status_low;
+
+  if(runadc){
+    #if _I2S_ADC == true
       readFiFo();
-#else
+    #else
       adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]);
       adc_read_index[0]++;
       if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0;
-#endif
-      }
+    #endif
+  }
+
   // low side
-  MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status;
+  MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status_low;
   // high side
-  // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0;
+  MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_high;
 }
 
 static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused));
 
 // Read currents when interrupt is triggered
 static void IRAM_ATTR mcpwm1_isr_handler(void*){
-  // // high side
-  // uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st;
+  // high side
+  uint32_t mcpwm_intr_status_high = MCPWM1.int_st.timer0_tez_int_st;
 
   // low side
-  uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st;
-  if(mcpwm_intr_status){
-#if _I2S_ADC == true
-    delayMicroseconds(2);
-    readFiFo();
-#else
-    adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]);
-    adc_read_index[1]++;
-    if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0;
-#endif
+  uint32_t mcpwm_intr_status_low = MCPWM1.int_st.timer0_tep_int_st;
+
+  bool runadc = mcpwm_intr_status_high || mcpwm_intr_status_low;
+
+  if(runadc){
+    #if _I2S_ADC == true
+      readFiFo();
+    #else
+      adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]);
+      adc_read_index[1]++;
+      if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0;
+    #endif
   }
   // low side
-  MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status;
+  MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status_low;
   // high side
-  // MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0;
+  MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_high;
 }
 
 

From ffcd9670d6e4663da654a4d10359a90a41b72e5c Mon Sep 17 00:00:00 2001
From: mcells <33664753+mcells@users.noreply.github.com>
Date: Fri, 29 Mar 2024 18:57:06 +0000
Subject: [PATCH 14/14] Stop reading samples during readfifo for better speed &
 timing

---
 .../hardware_specific/esp32/esp32_i2s_driver.cpp              | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
index 9a638024..006dfdc4 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
+++ b/src/current_sense/hardware_specific/esp32/esp32_i2s_driver.cpp
@@ -67,6 +67,8 @@ unsigned long IRAM_ATTR fifotime = 0;
 // The ADC counts get saved in uint32_t i2s_adc_buffer[].
 void IRAM_ATTR readFiFo()
 {
+    CLEAR_PERI_REG_MASK(I2S_CONF_REG(0), I2S_RX_START); // Stop aquisition to buffer
+
     // uint32_t readings[ADC1_CHANNEL_MAX][ADC1_CHANNEL_MAX*BUF_LEN];
     uint32_t avgreadings[ADC1_CHANNEL_MAX] = {0};
     uint32_t counts[ADC1_CHANNEL_MAX] = {0};
@@ -173,6 +175,8 @@ void IRAM_ATTR readFiFo()
           // Serial.printf(">Channel%d:%d\n", j, i2s_adc_buffer[j]);
       }
   }*/
+
+  SET_PERI_REG_MASK(I2S_CONF_REG(0), I2S_RX_START); // Restart aquisition to buffer
 }
 
 #if I2S_USE_INTERRUPT == true