From 2bc8d73776434bb2f15113014299a9fffa547109 Mon Sep 17 00:00:00 2001
From: Dan Halbert <halbert@halwitz.org>
Date: Tue, 16 Aug 2022 19:58:35 -0400
Subject: [PATCH] Fix ESP32 touch; rework common code a bit

---
 ports/espressif/common-hal/touchio/TouchIn.c | 24 +++-------------
 ports/espressif/peripherals/touch.c          | 30 ++++++++++++++------
 ports/espressif/peripherals/touch.h          |  1 +
 3 files changed, 27 insertions(+), 28 deletions(-)

diff --git a/ports/espressif/common-hal/touchio/TouchIn.c b/ports/espressif/common-hal/touchio/TouchIn.c
index 523245f359b86..c2ef0d6e187b7 100644
--- a/ports/espressif/common-hal/touchio/TouchIn.c
+++ b/ports/espressif/common-hal/touchio/TouchIn.c
@@ -30,22 +30,9 @@
 #include "peripherals/touch.h"
 #include "shared-bindings/microcontroller/Pin.h"
 
-static uint16_t get_raw_reading(touchio_touchin_obj_t *self) {
-    #if defined(CONFIG_IDF_TARGET_ESP32)
-    uint16_t touch_value;
-    #else
-    uint32_t touch_value;
-    #endif;
-    touch_pad_read_raw_data(self->pin->touch_channel, &touch_value);
-    if (touch_value > UINT16_MAX) {
-        return UINT16_MAX;
-    }
-    return (uint16_t)touch_value;
-}
-
 void common_hal_touchio_touchin_construct(touchio_touchin_obj_t *self,
     const mcu_pin_obj_t *pin) {
-    if (pin->touch_channel == TOUCH_PAD_MAX) {
+    if (pin->touch_channel == NO_TOUCH_CHANNEL) {
         raise_ValueError_invalid_pin();
     }
     claim_pin(pin);
@@ -53,15 +40,12 @@ void common_hal_touchio_touchin_construct(touchio_touchin_obj_t *self,
     // initialize touchpad
     peripherals_touch_init(pin->touch_channel);
 
-    // wait for touch data to reset
-    mp_hal_delay_ms(10);
-
     // Set a "touched" threshold not too far above the initial value.
     // For simple finger touch, the values may vary as much as a factor of two,
     // but for touches using fruit or other objects, the difference is much less.
 
     self->pin = pin;
-    self->threshold = get_raw_reading(self) + 100;
+    self->threshold = common_hal_touchio_touchin_get_raw_value(self) + 100;
 }
 
 bool common_hal_touchio_touchin_deinited(touchio_touchin_obj_t *self) {
@@ -77,11 +61,11 @@ void common_hal_touchio_touchin_deinit(touchio_touchin_obj_t *self) {
 }
 
 bool common_hal_touchio_touchin_get_value(touchio_touchin_obj_t *self) {
-    return get_raw_reading(self) > self->threshold;
+    return common_hal_touchio_touchin_get_raw_value(self) > self->threshold;
 }
 
 uint16_t common_hal_touchio_touchin_get_raw_value(touchio_touchin_obj_t *self) {
-    return get_raw_reading(self);
+    return peripherals_touch_read(self->pin->touch_channel);
 }
 
 uint16_t common_hal_touchio_touchin_get_threshold(touchio_touchin_obj_t *self) {
diff --git a/ports/espressif/peripherals/touch.c b/ports/espressif/peripherals/touch.c
index e656bbddf1f9c..9cf718b50160b 100644
--- a/ports/espressif/peripherals/touch.c
+++ b/ports/espressif/peripherals/touch.c
@@ -43,7 +43,7 @@ void peripherals_touch_never_reset(const bool enable) {
 void peripherals_touch_init(const touch_pad_t touchpad) {
     if (!touch_inited) {
         touch_pad_init();
-        touch_pad_set_fsm_mode(TOUCH_FSM_MODE_TIMER);
+        touch_pad_set_fsm_mode(TOUCH_FSM_MODE_SW);
     }
     // touch_pad_config() must be done before touch_pad_fsm_start() the first time.
     // Otherwise the calibration is wrong and we get maximum raw values if there is
@@ -52,13 +52,27 @@ void peripherals_touch_init(const touch_pad_t touchpad) {
     touch_pad_config(touchpad, 0);
     #else
     touch_pad_config(touchpad);
+    touch_pad_fsm_start();
     #endif
-    if (!touch_inited) {
-        #if defined(CONFIG_IDF_TARGET_ESP32)
-        touch_pad_sw_start();
-        #else
-        touch_pad_fsm_start();
-        #endif
-        touch_inited = true;
+    touch_inited = true;
+}
+
+uint16_t peripherals_touch_read(touch_pad_t touchpad) {
+    #if defined(CONFIG_IDF_TARGET_ESP32)
+    uint16_t touch_value;
+    touch_pad_read(touchpad, &touch_value);
+    // ESP32 touch_pad_read() returns a lower value when a pin is touched instead of a higher value.
+    // Flip the values around to be consistent with TouchIn assumptions.
+    return UINT16_MAX - touch_value;
+    #else
+    uint32_t touch_value;
+    touch_pad_sw_start();
+    while (!touch_pad_meas_is_done()) {
+    }
+    touch_pad_read_raw_data(touchpad, &touch_value);
+    if (touch_value > UINT16_MAX) {
+        return UINT16_MAX;
     }
+    return (uint16_t)touch_value;
+    #endif
 }
diff --git a/ports/espressif/peripherals/touch.h b/ports/espressif/peripherals/touch.h
index c7cd1f6be1e8e..5feba4410a0dc 100644
--- a/ports/espressif/peripherals/touch.h
+++ b/ports/espressif/peripherals/touch.h
@@ -29,6 +29,7 @@
 
 #include "driver/touch_pad.h"
 
+extern uint16_t peripherals_touch_read(touch_pad_t touchpad);
 extern void peripherals_touch_reset(void);
 extern void peripherals_touch_never_reset(const bool enable);
 extern void peripherals_touch_init(const touch_pad_t touchpad);