Add endstop debouncing

Firmware/MotorControl/axis.cpp

@@ -140,6 +140,8 @@ bool Axis::do_updates() {
     // Sub-components should use set_error which will propegate to this error_
     encoder_.update();
     sensorless_estimator_.update();
+    min_endstop_.update();
+    max_endstop_.update();
     return check_for_errors();
 }
 
@@ -214,20 +216,20 @@ bool Axis::run_closed_loop_control_loop() {
 
         // Handle the homing case
         if (homing_state_ == HOMING_STATE_HOMING) {
-            if (min_endstop_.endstop_state_) {
+            if (min_endstop_.getEndstopState()) {
                 encoder_.set_linear_count(min_endstop_.config_.offset);
                 controller_.set_pos_setpoint(0.0f, 0.0f, 0.0f);
                 homing_state_ = HOMING_STATE_MOVE_TO_ZERO;
             }
         } else if (homing_state_ == HOMING_STATE_MOVE_TO_ZERO) {
-            if(!min_endstop_.endstop_state_){
+            if(!min_endstop_.getEndstopState()){
                 homing_state_ = HOMING_STATE_IDLE;
             }
         } else {
             // Check for endstop presses
-            if (min_endstop_.config_.enabled && min_endstop_.endstop_state_) {
+            if (min_endstop_.config_.enabled && min_endstop_.getEndstopState()) {
                 return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false;
-            } else if (max_endstop_.config_.enabled && max_endstop_.endstop_state_) {
+            } else if (max_endstop_.config_.enabled && max_endstop_.getEndstopState()) {
                 return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false;
             }
         }

Firmware/MotorControl/endstop.cpp

@@ -1,4 +1,5 @@
 #include <odrive_main.h>
+#include <algorithm>
 
 Endstop::Endstop(EndstopConfig_t &config)
     : config_(config) {
@@ -8,19 +9,32 @@ static void endstop_cb_wrapper(void* ctx){
     reinterpret_cast<Endstop*>(ctx)->endstop_cb();
 }
 
-void Endstop::endstop_cb(){
-    uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
-    GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
-
-    if(config_.enabled){
-        endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
-        if(config_.is_active_high == false)
-            endstop_state_ = !endstop_state_;
+void Endstop::update() {
+    if (config_.enabled) {
+        float now = axis_->loop_counter_ * current_meas_period;
+        if((now - debounce_timer_) >= config_.debounce_ms) {                        // Debounce timer expired, take the new pin state
+            endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_;     // endstop_state is the logical state
+            debounce_timer_ = now - config_.debounce_ms;                            // Ensure timer doesn't have overflow issues
+        } else {
+            endstop_state_ = endstop_state_; // Do nothing
+        }
     } else {
         endstop_state_ = false;
     }
 }
 
+bool Endstop::getEndstopState() {
+    return endstop_state_;
+}
+
+void Endstop::endstop_cb() {
+    uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
+    GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
+
+    debounce_timer_ = axis_->loop_counter_ * current_meas_period;
+    pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
+}
+
 void Endstop::set_endstop_enabled(bool enable){
     uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
     GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);

Firmware/MotorControl/endstop.hpp

@@ -7,6 +7,7 @@ struct EndstopConfig_t {
     bool enabled = false;
     int32_t offset = 0;
     bool is_active_high = false;
+    float debounce_ms = 100;
 };
 
 class Endstop {
@@ -15,10 +16,11 @@ class Endstop {
     EndstopConfig_t config_;
     Axis* axis_ = nullptr;
 
-    bool endstop_state_ = false;
-
     void set_endstop_enabled(bool enable);
     void endstop_cb();
+    void update();
+
+    bool getEndstopState();
 
     auto make_protocol_definitions(){
         return make_protocol_member_list(
@@ -26,12 +28,15 @@ class Endstop {
                                  make_protocol_property("gpio_num", &config_.gpio_num),
                                  make_protocol_property("enabled", &config_.enabled),
                                  make_protocol_property("offset", &config_.offset),
-                                 make_protocol_property("is_active_high", &config_.is_active_high)
+                                 make_protocol_property("is_active_high", &config_.is_active_high),
+                                 make_protocol_property("debounce_ms", &config_.debounce_ms)
                                 )
         );
     }
 
    private:
-    uint16_t debounce_timer_ = 0;
+    bool endstop_state_ = false;
+    bool pin_state_ = false;
+    float debounce_timer_ = 0;
 };
 #endif