Add ESP32 Hardware Encoder Class

src/encoders/esp32hwencoder/ESP32HWEncoder.cpp

@@ -0,0 +1,198 @@
+#include "ESP32HWEncoder.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
+
+
+
+ESP32HWEncoder::ESP32HWEncoder(int pinA, int pinB, int32_t ppr, int pinI)
+{
+    #ifdef USE_ARDUINO_PINOUT
+        // Handle Arduino Nano ESP32 quirks with the pin assignments
+        _pinA = digitalPinToGPIO(pinA);
+        _pinB = digitalPinToGPIO(pinB);
+        _pinI = digitalPinToGPIO(pinI);
+    #else
+        _pinA = pinA;
+        _pinB = pinB;
+        _pinI = pinI;
+    #endif
+
+    cpr = ppr * 4; // 4x for quadrature
+
+    pcnt_config.ctrl_gpio_num =  _pinA;
+    pcnt_config.pulse_gpio_num = _pinB;
+    pcnt_config.counter_l_lim = INT16_MIN;
+    pcnt_config.counter_h_lim = INT16_MAX;
+} 
+
+// Interrupt handler for accumulating the pulsecounter count
+void IRAM_ATTR overflowCounter(void* arg)
+{
+    uint8_t interruptStatus = PCNT.int_st.val;
+    for (uint32_t i = 0; i < 8; i++)
+    {
+        if (interruptStatus & BIT(i))
+        {
+            int32_t set = ((overflowISR_args_t*) arg)[i].set;
+            if(set != 1){ continue;}
+            int32_t* count = ((overflowISR_args_t*) arg)[i].angleoverflow_val;
+
+            // Add or subtract depending on the direction of the overflow
+            switch (PCNT.status_unit[i].val)
+            {
+            case PCNT_EVT_L_LIM:
+                *count += INT16_MIN;
+                break;
+            case PCNT_EVT_H_LIM:
+                *count += INT16_MAX;
+                break;
+            default:
+                break;
+            }
+
+            // Clear the interrupt
+            PCNT.int_clr.val = BIT(i);
+        }
+    }
+}
+
+// Interrupt handler for zeroing the pulsecounter count
+void IRAM_ATTR ESP32HWEncoder::indexHandler()
+    {
+        pcnt_counter_clear(pcnt_config.unit);
+        angleOverflow = 0;
+        indexFound = true;
+    }
+
+void ESP32HWEncoder::init()
+{
+
+    // Statically allocate and initialize the spinlock
+    spinlock = portMUX_INITIALIZER_UNLOCKED;
+
+    // find a free pulsecount unit
+    for (uint8_t i = 0; i < PCNT_UNIT_MAX; i++)
+    {
+        if(cpr > 0){
+            inv_cpr = 1.0f/cpr;
+        }
+        if(used_units[i] == 0){
+            pcnt_config.unit = (pcnt_unit_t) i;
+            if(pcnt_unit_config(&pcnt_config) == ESP_OK){
+                initialized = true;
+                // Serial.printf("Configured PCNT unit %d\n", i);
+                used_units[i] = 1;
+                break;
+            }
+        }
+
+    }
+    if (initialized)
+    {
+        // Set up the PCNT peripheral
+        pcnt_set_pin(pcnt_config.unit, PCNT_CHANNEL_0, pcnt_config.ctrl_gpio_num, pcnt_config.pulse_gpio_num);
+        pcnt_set_pin(pcnt_config.unit, PCNT_CHANNEL_1, pcnt_config.pulse_gpio_num, pcnt_config.ctrl_gpio_num);
+        pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DEC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
+        pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DEC, PCNT_COUNT_INC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
+
+        pcnt_counter_pause(pcnt_config.unit);
+        pcnt_counter_clear(pcnt_config.unit);
+
+        // Select interrupt on reaching high and low counter limit
+        pcnt_event_enable(pcnt_config.unit, PCNT_EVT_L_LIM);
+        pcnt_event_enable(pcnt_config.unit, PCNT_EVT_H_LIM);
+
+        // Pass pointer to the angle accumulator and the current PCNT unit to the ISR
+        overflowISR_args[pcnt_config.unit].angleoverflow_val = &angleOverflow;
+        overflowISR_args[pcnt_config.unit].unit = pcnt_config.unit;
+        overflowISR_args[pcnt_config.unit].set = 1;
+
+        // Register and enable the interrupt
+        pcnt_isr_register(overflowCounter, (void*)&overflowISR_args, 0, (pcnt_isr_handle_t*) NULL);
+        pcnt_intr_enable(pcnt_config.unit);
+
+        // Just check the last command for errors
+        if(pcnt_counter_resume(pcnt_config.unit) != ESP_OK){
+            initialized = false;
+        }
+
+        // If an index Pin is defined, create an ISR to zero the angle when the index fires
+        if (hasIndex())
+        {
+            attachInterrupt(static_cast<u_int8_t>(_pinI), std::bind(&ESP32HWEncoder::indexHandler,this), RISING);
+        }
+
+        // Optionally use pullups
+        if (pullup == USE_INTERN)
+        {
+            pinMode(static_cast<u_int8_t>(_pinA), INPUT_PULLUP);
+            pinMode(static_cast<u_int8_t>(_pinB), INPUT_PULLUP);
+            if (hasIndex()) {pinMode(static_cast<u_int8_t>(_pinI), INPUT_PULLUP);}
+        }
+
+    }
+
+}
+
+int ESP32HWEncoder::needsSearch()
+{
+        return !((indexFound && hasIndex()) || !hasIndex());
+}
+
+int ESP32HWEncoder::hasIndex()
+{
+    return _pinI != -1;
+}
+
+void ESP32HWEncoder::setCpr(int32_t ppr){
+    cpr = 4*ppr;
+    if(cpr > 0){
+        inv_cpr = 1.0f/cpr; // Precalculate the inverse of cpr to avoid "slow" float divisions
+    }
+}
+
+int32_t ESP32HWEncoder::getCpr(){
+    return cpr;
+}
+
+// Change to Step/Dir counting mode. A->Step, B->Dir
+void ESP32HWEncoder::setStepDirMode(){
+    pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_KEEP);
+    pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DIS, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_REVERSE);
+}
+
+// Change to default AB (quadrature) mode
+void ESP32HWEncoder::setQuadratureMode(){
+    pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_0, PCNT_COUNT_INC, PCNT_COUNT_DEC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
+    pcnt_set_mode(pcnt_config.unit, PCNT_CHANNEL_1, PCNT_COUNT_DEC, PCNT_COUNT_INC, PCNT_MODE_REVERSE, PCNT_MODE_KEEP);
+}
+
+float IRAM_ATTR ESP32HWEncoder::getSensorAngle()
+{
+    if(!initialized){return -1.0f;}
+
+    taskENTER_CRITICAL(&spinlock);
+    // We are now in a critical section to prevent interrupts messing with angleOverflow and angleCounter
+
+    // Retrieve the count register into a variable
+    pcnt_get_counter_value(pcnt_config.unit, &angleCounter);
+
+    // Trim the accumulator variable to prevent issues with it overflowing
+    // Make the % operand behave mathematically correct (-5 modulo 4 == 3; -5 % 4 == -1)
+    angleOverflow %= cpr;
+    if (angleOverflow < 0){
+        angleOverflow += cpr;
+    }
+
+    angleSum = (angleOverflow + angleCounter) % cpr;
+    if (angleSum < 0) {
+        angleSum += cpr;
+    }
+
+    taskEXIT_CRITICAL(&spinlock); // Exit critical section
+
+    // Calculate the shaft angle
+    return _2PI * angleSum * inv_cpr;
+}
+
+#endif

src/encoders/esp32hwencoder/ESP32HWEncoder.h

@@ -0,0 +1,66 @@
+
+#pragma once
+
+#include <Arduino.h>
+
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32)
+
+#include "driver/pcnt.h"
+#include "soc/pcnt_struct.h"
+#include "common/base_classes/Sensor.h"
+#include "common/foc_utils.h"
+#include "FunctionalInterrupt.h"
+
+static struct overflowISR_args_t {
+    int32_t* angleoverflow_val;
+    pcnt_unit_t unit;
+    int32_t set;
+}overflowISR_args[PCNT_UNIT_MAX];
+
+// Statically allocate and initialize a spinlock
+static portMUX_TYPE spinlock;
+static int used_units[PCNT_UNIT_MAX];
+
+class ESP32HWEncoder : public Sensor{
+    public:
+        /**
+        Encoder class constructor
+        @param ppr  impulses per rotation  (cpr=ppr*4)
+        */
+        explicit ESP32HWEncoder(int pinA, int pinB, int32_t ppr, int pinI=-1);
+
+        void init() override;
+        int needsSearch() override;
+        int hasIndex();
+        float getSensorAngle() override;
+        void setCpr(int32_t ppr);
+        int32_t getCpr();
+        void setStepDirMode();
+        void setQuadratureMode();
+        bool initialized = false;
+
+        Pullup pullup; //!< Configuration parameter internal or external pullups
+
+
+
+    protected:
+
+
+        void IRAM_ATTR indexHandler();
+
+        bool indexFound = false;
+
+        int _pinA, _pinB, _pinI;
+
+        pcnt_config_t pcnt_config;
+
+        int16_t angleCounter; // Stores the PCNT value
+        int32_t angleOverflow; // In case the PCNT peripheral overflows, this accumulates the max count to keep track of large counts/angles (>= 16 Bit). On index, this gets reset.
+        int32_t angleSum; // Sum of Counter and Overflow in range [0,cpr]
+
+        int32_t cpr; // Counts per rotation = 4 * ppr for quadrature encoders
+        float inv_cpr;
+};
+
+#endif

src/encoders/esp32hwencoder/README.md

@@ -0,0 +1,49 @@
+# SimpleFOC Driver for ESP32 hardware encoder
+
+This encoder driver uses the ESP32´s dedicated pulse counter hardware to efficiently count the AB(I) signals of an encoder. It also supports a Step/Dir-type mode.
+
+Because most of the counting is done by the peripheral, it should support much higher speeds in comparison to the generic interrupt-based encoder implementation provided in the main library. 
+The PCNT peripheral can count at several MHz and should not be a limiting factor here.
+
+You can use encoders with cpr of up to 31 bits. (At this resolution, you would get about 100 counts per second if you mounted such a sensor on the earths rotational axis. Thats plenty ;-) ) 
+
+
+## Status
+
+Seems to work fine! Step/Dir mode is untested.
+
+## Hardware Setup
+
+You can connect the encoder to any digital input pin of the ESP32, as they all support the PCNT peripheral.
+
+## Configuration
+
+This is a near drop-in replacement for the standard encoder class:
+
+```c++
+#include "Arduino.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/esp32hwencoder/ESP32HWEncoder.h"
+
+#define ENCODER_PPR 4711
+#define ENCODER_PIN_A 16
+#define ENCODER_PIN_B 17
+#define ENCODER_PIN_I 21
+
+ESP32HWEncoder encoder = ESP32HWEncoder(ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PPR, ENCODER_PIN_I); // The Index pin can be omitted
+
+void setup() {
+    encoder.pullup = Pullup::USE_INTERN; // optional: pullups
+
+    encoder.setStepDirMode(); // optional: set Stepper type step/dir mode
+
+    encoder.init();
+}
+
+void loop() {
+    encoder.update(); // optional: Update manually if not using loopfoc()
+
+    encoder.getAngle() // Access the sensor value
+}
+```