Fixed and touch is stable.

BertFPVDiversity.ino

@@ -471,10 +471,16 @@ void setup_webserver(void) {
 }
 
 
-const int VALUE_THRESHOLD = 1;
+const int THRESHOLD_MODE = 10;
+const int THRESHOLD_DOWN = 20;
+const int THRESHOLD_UP = 10;
+const int THRESHOLD_ENTER = 10;
 int touched_val = 0;
 bool touched = false;
-uint32_t last_touched = 0;
+uint32_t last_mode_touched = 0;
+uint32_t last_prev_touched = 0;
+uint32_t last_next_touched = 0;
+uint32_t last_enter_touched = 0;
 uint32_t current_millis = 0;
 
 hw_timer_t * timer = NULL;      /* create a hardware timer */
@@ -569,8 +575,10 @@ void touchBeep(uint8_t beep_type = 0) {
     // Instead of delay we use cronjob method
     if(beep_type == 0) {
         last_buzzer_seconds = millis() + 200;
-    } else {
+    } else if(beep_type == 1) {
         last_buzzer_seconds = millis() + 500;
+    } else if(beep_type == 2) {
+        last_buzzer_seconds = millis() + 100;
     }
     last_buzzer_seconds_cron = true;
 }
@@ -662,6 +670,41 @@ void osd(uint8_t state) {
     }
 }
 
+bool touchpad(uint8_t touch_id) {
+    uint32_t total_reads = 0;
+    uint8_t max_reads = 10;
+    uint32_t average_reads = 0;
+
+    for(int read_counts = 0; read_counts < max_reads; read_counts++) {
+        touched_val = touchRead(touch_id);
+        total_reads += touched_val;
+    }
+    average_reads = total_reads / max_reads;
+
+    if(touch_id == TOUCH_MODE) {
+        if(average_reads < THRESHOLD_MODE)
+            return true;
+        else
+            return false;
+    } else if(touch_id == TOUCH_DOWN) {
+        Serial.println(average_reads);
+        if(average_reads < THRESHOLD_DOWN)
+            return true;
+        else
+            return false;
+    } else if(touch_id == TOUCH_UP) {
+        if(average_reads < THRESHOLD_UP)
+            return true;
+        else
+            return false;
+    } else if(touch_id == TOUCH_ENTER) {
+        if(average_reads < THRESHOLD_ENTER)
+            return true;
+        else
+            return false;
+    }
+}
+
 void loop() {
     // Handling web services every 100mS
     if(millis() > last_webserver_seconds) {
@@ -680,9 +723,8 @@ void loop() {
         display_output();
 
     // Checking TouchPad - MODE
-    touched_val = touchRead(TOUCH_MODE);
-    if(touched_val < VALUE_THRESHOLD) {
-        if(millis() > last_touched) {
+    if(millis() > last_mode_touched) {
+        if(touchpad(TOUCH_MODE)) {
             display_output_state = !display_output_state;
             if(!display_output_state) {
                 osd(0);
@@ -690,16 +732,71 @@ void loop() {
                 osd(1);
             }
 
-            last_touched = millis() + 500;
+            // Next touch scan
+            last_mode_touched = millis() + 500;
+        } else {
+            // Next touch scan
+            last_mode_touched = millis() + 100;
+        }
+    }
+
+    // Checking TouchPad - NEXT
+    if(millis() > last_next_touched) {
+        if(touchpad(TOUCH_DOWN)) {
+            Menu::menu_position++;
+            Menu::menu_position = (Menu::menu_position >= Menu::MAX_MENU_ITEMS) ? 0 : Menu::menu_position;
+            touchBeep(2);
+
+            // Next touch scan
+            last_next_touched = millis() + 500;
+        } else {
+            // Next touch scan
+            last_next_touched = millis() + 100;
+        }
+    }
+
+    // Checking TouchPad - PREV
+    if(millis() > last_prev_touched) {
+        if(touchpad(TOUCH_UP)) {
+            Menu::menu_position--;
+            Menu::menu_position = (Menu::menu_position < 0) ? Menu::MAX_MENU_ITEMS - 1 : Menu::menu_position;
+            touchBeep(2);
+
+            // Next touch scan
+            last_next_touched = millis() + 500;
+        } else {
+            // Next touch scan
+            last_next_touched = millis() + 100;
+        }
+    }
+
+    // Checking TouchPad - ENTER
+    if(millis() > last_enter_touched) {
+        if(touchpad(TOUCH_ENTER)) {
+            touchBeep(2);
+
+            // Different menu different functions
+            switch(Menu::menu_position) {
+                case 3:
+                    delay(800);
+                    ESP.restart();
+                    break;
+            }
+
+            // Next touch scan
+            last_enter_touched = millis() + 500;
+        } else {
+            // Next touch scan
+            last_enter_touched = millis() + 100;
         }
     }
 /****************
     // Checking TouchPad - Up
     touched_val = touchRead(TOUCH_UP);
-    if(touched_val < VALUE_THRESHOLD) {
-        if(millis() > last_touched) {
+    if(touched_val < THRESHOLD_MODE) {
+        if(millis() > last_mode_touched) {
             Menu::menu_position--;
-            last_touched = millis() + 500;
+            last_mode_touched = millis() + 500;
             Menu::menu_position = (Menu::menu_position < 0) ? Menu::MAX_MENU_ITEMS - 1 : Menu::menu_position;
             //Menu::menu_position = (Menu::menu_position >= Menu::MAX_MENU_ITEMS) ? 0 : Menu::menu_position;
         }
@@ -707,10 +804,10 @@ void loop() {
 
     // Checking TouchPad - DOWN
     touched_val = touchRead(TOUCH_DOWN);
-    if(touched_val < VALUE_THRESHOLD) {
-        if(millis() > last_touched) {
+    if(touched_val < THRESHOLD_MODE) {
+        if(millis() > last_mode_touched) {
             Menu::menu_position++;
-            last_touched = millis() + 500;
+            last_mode_touched = millis() + 500;
             //Menu::menu_position = (Menu::menu_position < 0) ? Menu::MAX_MENU_ITEMS - 1 : Menu::menu_position;
             Menu::menu_position = (Menu::menu_position >= Menu::MAX_MENU_ITEMS) ? 0 : Menu::menu_position;
         }