hardware-mapping: debounce inputs in TestSwitch()

Signed-off-by: H Hartley Sweeten <hsweeten@visionengravers.com>

src/gcode-machine-control.cc

@@ -384,14 +384,14 @@ void GCodeMachineControl::Impl::wait_for_start() {
   if (pause_enabled_ && check_for_pause()) {
     mprintf("// BeagleG: pause switch active\n");
     int pause_active = PAUSE_ACTIVE_DETECT;
+    // The switch reading is debounced. We add additional delay with
+    // the while loop to ensure that the pause switch has been cleared.
     while (pause_active) {
       usleep(1000);
-      // TODO: we should probably have de-bouncing logic rather in the
-      // hardware mapping. E.g. something like TestPauseSwitch(2000).
       if (check_for_pause())
-	pause_active = PAUSE_ACTIVE_DETECT;
+        pause_active = PAUSE_ACTIVE_DETECT;
       else
-	pause_active--;
+        pause_active--;
     }
     mprintf("// BeagleG: pause switch cleared\n");
   }

src/hardware-mapping.cc

@@ -305,9 +305,21 @@ HardwareMapping::AxisTrigger HardwareMapping::AvailableAxisSwitch(LogicAxis axis
 bool HardwareMapping::TestSwitch(const int switch_number, bool def_result) {
   if (!is_hardware_initialized_) return def_result;
   GPIODefinition gpio_def = get_endstop_gpio_descriptor(switch_number);
-  if (gpio_def != GPIO_NOT_MAPPED)
-    return (get_gpio(gpio_def) == trigger_level_[switch_number-1]);
-  return def_result;
+  if (gpio_def == GPIO_NOT_MAPPED) return def_result;
+  bool state = get_gpio(gpio_def);
+  int debounce = 0;
+  for (;;) {
+    usleep(10);
+    bool new_state = get_gpio(gpio_def);
+    if (new_state == state) {
+      debounce++;
+      if (debounce == 2) break;
+    } else {
+      state = new_state;
+      debounce = 0;
+    }
+  }
+  return (state == trigger_level_[switch_number-1]);
 }
 
 bool HardwareMapping::TestAxisSwitch(LogicAxis axis, AxisTrigger requested_trigger) {