[homing] make separate control mode

[endstops] enable in all control modes [load encoder] use load encoder more consequently
2026-09-21 23:44:48 +08:00 · 2019-09-23 12:23:27 +02:00
parent 9e782cce7b
commit f8ea7d5684
11 changed files with 242 additions and 166 deletions
@@ -33,10 +33,10 @@ Axis::Axis(int axis_num,
    controller_.axis_ = this;
    motor_.axis_ = this;
    trap_.axis_ = this;
-    decode_step_dir_pins();
-    watchdog_feed();
    min_endstop_.axis_ = this;
    max_endstop_.axis_ = this;
+    decode_step_dir_pins();
+    watchdog_feed();
 }

 Axis::LockinConfig_t Axis::default_calibration() {
@@ -182,10 +182,18 @@ bool Axis::do_checks() {

    // Sub-components should use set_error which will propegate to this error_
    motor_.do_checks();
-    encoder_.do_checks();
+    // encoder_.do_checks();
    // sensorless_estimator_.do_checks();
    // controller_.do_checks();

+    // Check for endstop presses
+    bool vel_dependent_stopping = (current_state_ == AXIS_STATE_HOMING) && (controller_.config_.control_mode >= Controller::CTRL_MODE_VELOCITY_CONTROL);
+    if (min_endstop_.config_.enabled && min_endstop_.get_state() && (!vel_dependent_stopping || controller_.vel_setpoint_ < 0.0f)) {
+        error_ |= ERROR_MIN_ENDSTOP_PRESSED;
+    } else if (max_endstop_.config_.enabled && max_endstop_.get_state() && (!vel_dependent_stopping || controller_.vel_setpoint_ > 0.0f)) {
+        error_ |= ERROR_MAX_ENDSTOP_PRESSED;
+    }
+
    return check_for_errors();
 }

@@ -287,13 +295,15 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {

 // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
 bool Axis::run_sensorless_control_loop() {
-    run_control_loop([this](){
-        if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
-            return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
+    controller_.pos_estimate_src_ = nullptr;
+    controller_.pos_estimate_valid_src_ = nullptr;
+    controller_.vel_estimate_src_ = &sensorless_estimator_.vel_estimate_;
+    controller_.vel_estimate_valid_src_ = &sensorless_estimator_.vel_estimate_valid_;

+    run_control_loop([this](){
        // Note that all estimators are updated in the loop prefix in run_control_loop
        float current_setpoint;
-        if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
+        if (!controller_.update(&current_setpoint))
            return error_ |= ERROR_CONTROLLER_FAILED, false;
        if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
            return false; // set_error should update axis.error_
@@ -303,8 +313,12 @@ bool Axis::run_sensorless_control_loop() {
 }

 bool Axis::run_closed_loop_control_loop() {
+    if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
+        return error_ |= ERROR_CONTROLLER_FAILED, false;
+    }
+
    // To avoid any transient on startup, we intialize the setpoint to be the current position
-    controller_.pos_setpoint_ = encoder_.pos_estimate_;
+    controller_.pos_setpoint_ = *controller_.pos_estimate_src_;

    // Avoid integrator windup issues
    controller_.vel_integrator_current_ = 0.0f;
@@ -313,62 +327,99 @@ bool Axis::run_closed_loop_control_loop() {
    run_control_loop([this](){
        // Note that all estimators are updated in the loop prefix in run_control_loop
        float current_setpoint;
-        if (controller_.config_.use_load_encoder) {
-            if (controller_.config_.load_encoder_axis < AXIS_COUNT) {
-                Axis* ax = axes[controller_.config_.load_encoder_axis];
-                if (!controller_.update(ax->encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
-                    return error_ |= ERROR_CONTROLLER_FAILED, false;
-            } else{
-                controller_.set_error(Controller::ERROR_INVALID_LOAD_ENCODER);
-                return error_ |= ERROR_CONTROLLER_FAILED, false;
-            }
-        } else if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
-            return error_ |= ERROR_CONTROLLER_FAILED, false;  //TODO: Make controller.set_error
+        if (!controller_.update(&current_setpoint))
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+
        float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
        if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
            return false; // set_error should update axis.error_

-        // Handle the homing case
-        if (homing_.homing_state == HOMING_STATE_HOMING) {
-            if (min_endstop_.getEndstopState()) {
-                // pos_setpoint is the starting position for the trap_traj so we need to set it.
-                controller_.pos_setpoint_ = min_endstop_.config_.offset;
-                controller_.vel_setpoint_ = 0.0f;  // Change directions without decelerating
-
-                // Set our current position in encoder counts to make control more logical
-                encoder_.set_linear_count(static_cast<int32_t>(controller_.pos_setpoint_));
-
-                controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
-                controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
-
-                controller_.input_pos_ = 0.0f;
-                controller_.input_pos_updated();
-                controller_.input_vel_ = 0.0f;
-                controller_.input_current_ = 0.0f;
-
-                homing_.homing_state = HOMING_STATE_MOVE_TO_ZERO;
-            }
-        } else if (homing_.homing_state == HOMING_STATE_MOVE_TO_ZERO) {
-            if(!min_endstop_.getEndstopState() && controller_.trajectory_done_){
-                controller_.config_.control_mode = homing_.storedControlMode;
-                controller_.config_.input_mode = homing_.storedInputMode;
-                homing_.homing_state = HOMING_STATE_IDLE;
-                homing_.isHomed = true;
-            }
-        } else {
-            // Check for endstop presses
-            if (min_endstop_.config_.enabled && min_endstop_.getEndstopState()) {
-                return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false;
-            } else if (max_endstop_.config_.enabled && max_endstop_.getEndstopState()) {
-                return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false;
-            }
-        }
        return true;
    });
    set_step_dir_active(false);
    return check_for_errors();
 }

+
+// Slowly drive in the negative direction at homing_speed until the min endstop is pressed
+// When pressed, set the linear count to the offset (default 0), and then go to position 0
+bool Axis::run_homing() {
+    Controller::ControlMode_t stored_control_mode = controller_.config_.control_mode;
+    Controller::InputMode_t stored_input_mode = controller_.config_.input_mode;
+
+    if (!min_endstop_.config_.enabled) {
+        return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false; // TODO: define new error code
+    }
+
+    controller_.config_.control_mode = Controller::CTRL_MODE_VELOCITY_CONTROL;
+    controller_.config_.input_mode = Controller::INPUT_MODE_VEL_RAMP;
+
+    controller_.input_pos_ = 0.0f;
+    controller_.input_pos_updated();
+    controller_.input_vel_ = -controller_.config_.homing_speed;
+    controller_.input_current_ = 0.0f;
+
+    homing_.is_homed = false;
+
+    if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
+        return error_ |= ERROR_CONTROLLER_FAILED, false;
+    }
+    
+    // To avoid any transient on startup, we intialize the setpoint to be the current position
+    controller_.pos_setpoint_ = *controller_.pos_estimate_src_;
+
+    // Avoid integrator windup issues
+    controller_.vel_integrator_current_ = 0.0f;
+
+    run_control_loop([this](){
+        // Note that all estimators are updated in the loop prefix in run_control_loop
+        float current_setpoint;
+        if (!controller_.update(&current_setpoint))
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+
+        float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
+        if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
+            return false; // set_error should update axis.error_
+
+        return !min_endstop_.get_state();
+    });
+    error_ &= ~ERROR_MIN_ENDSTOP_PRESSED; // clear this error since we deliberately drove into the endstop
+
+    // pos_setpoint is the starting position for the trap_traj so we need to set it.
+    controller_.pos_setpoint_ = min_endstop_.config_.offset;
+    controller_.vel_setpoint_ = 0.0f;  // Change directions without decelerating
+
+    // Set our current position in encoder counts to make control more logical
+    encoder_.set_linear_count(static_cast<int32_t>(controller_.pos_setpoint_));
+
+    controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
+    controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
+
+    controller_.input_pos_ = 0.0f;
+    controller_.input_pos_updated();
+    controller_.input_vel_ = 0.0f;
+    controller_.input_current_ = 0.0f;
+
+    run_control_loop([this](){
+        // Note that all estimators are updated in the loop prefix in run_control_loop
+        float current_setpoint;
+        if (!controller_.update(&current_setpoint))
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+
+        float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
+        if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
+            return false; // set_error should update axis.error_
+
+        return !controller_.trajectory_done_;
+    });
+
+    controller_.config_.control_mode = stored_control_mode;
+    controller_.config_.input_mode = stored_input_mode;
+    homing_.is_homed = true;
+
+    return check_for_errors();
+}
+
 bool Axis::run_idle_loop() {
    // run_control_loop ignores missed modulation timing updates
    // if and only if we're in AXIS_STATE_IDLE
@@ -396,11 +447,10 @@ void Axis::run_state_machine_loop() {
                    task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
                if (config_.startup_encoder_offset_calibration)
                    task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
-                if (config_.startup_closed_loop_control){
-                    if(config_.startup_homing)
-                        task_chain_[pos++] = AXIS_STATE_HOMING;
+                if (config_.startup_homing)
+                    task_chain_[pos++] = AXIS_STATE_HOMING;
+                if (config_.startup_closed_loop_control)
                    task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
-                }
                else if (config_.startup_sensorless_control)
                    task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
                task_chain_[pos++] = AXIS_STATE_IDLE;
@@ -446,9 +496,9 @@ void Axis::run_state_machine_loop() {
                status = encoder_.run_direction_find();
            } break;

-            case AXIS_STATE_HOMING:
-                status = controller_.home_axis();
-                break;
+            case AXIS_STATE_HOMING: {
+                status = run_homing();
+            } break;

            case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
                if (!motor_.is_calibrated_)
@@ -5,13 +5,6 @@
 #error "This file should not be included directly. Include odrive_main.h instead."
 #endif

-    
-enum HomingState_t {
-    HOMING_STATE_IDLE,
-    HOMING_STATE_HOMING,
-    HOMING_STATE_MOVE_TO_ZERO
-};
-
 class Axis {
 public:
    enum Error_t {
@@ -26,7 +19,7 @@ public:
        ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80,
        ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
        ERROR_CONTROLLER_FAILED = 0x200,
-        ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
+        ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, // DEPRECATED
        ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
        ERROR_MIN_ENDSTOP_PRESSED = 0x1000,
        ERROR_MAX_ENDSTOP_PRESSED = 0x2000,
@@ -96,10 +89,7 @@ public:
    };

    struct Homing_t {
-        HomingState_t homing_state = HOMING_STATE_IDLE;
-        Controller::ControlMode_t storedControlMode = Controller::CTRL_MODE_POSITION_CONTROL;
-        Controller::InputMode_t storedInputMode = Controller::INPUT_MODE_PASSTHROUGH;
-        bool isHomed = false;
+        bool is_homed = false;
    };

    enum thread_signals {
@@ -224,6 +214,7 @@ public:
    bool run_lockin_spin(const LockinConfig_t &lockin_config);
    bool run_sensorless_control_loop();
    bool run_closed_loop_control_loop();
+    bool run_homing();
    bool run_idle_loop();

    constexpr uint32_t get_watchdog_reset() {
@@ -277,8 +268,7 @@ public:
            make_protocol_property("requested_state", &requested_state_),
            make_protocol_ro_property("loop_counter", &loop_counter_),
            make_protocol_ro_property("lockin_state", &lockin_state_),
-            make_protocol_ro_property("homing_state", &homing_.homing_state),
-            make_protocol_property("is_homed", &homing_.isHomed),
+            make_protocol_property("is_homed", &homing_.is_homed),
            make_protocol_object("config",
                                 make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
                                 make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
@@ -29,6 +29,25 @@ void Controller::input_pos_updated() {
    input_pos_updated_ = true;
 }

+bool Controller::select_encoder(size_t encoder_num) {
+    if (encoder_num < AXIS_COUNT) {
+        Axis* ax = axes[encoder_num];
+        if (config_.setpoints_in_cpr) {
+            pos_estimate_src_ = &ax->encoder_.pos_cpr_;
+            pos_wrap_src_ = &ax->encoder_.config_.cpr;
+        } else {
+            pos_estimate_src_ = &ax->encoder_.pos_estimate_;
+            pos_wrap_src_ = nullptr;
+        }
+        pos_estimate_valid_src_ = &ax->encoder_.pos_estimate_valid_;
+        vel_estimate_src_ = &ax->encoder_.vel_estimate_;
+        vel_estimate_valid_src_ = &ax->encoder_.vel_estimate_valid_;
+        return true;
+    } else {
+        return set_error(Controller::ERROR_INVALID_LOAD_ENCODER), false;
+    }
+}
+
 void Controller::move_to_pos(float goal_point) {
    axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
                                 axis_->trap_.config_.vel_limit,
@@ -55,30 +74,6 @@ void Controller::start_anticogging_calibration() {
    }
 }

-// Slowly drive in the negative direction at homing_speed until the min endstop is pressed
-// When pressed, set the linear count to the offset (default 0), and then
-
-//TODO: This needs to be upgraded to use its own run_control_loop!
-bool Controller::home_axis() {
-    if (axis_->min_endstop_.config_.enabled) {
-        axis_->homing_.storedControlMode = config_.control_mode;
-        axis_->homing_.storedInputMode = config_.input_mode;
-
-        config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
-        config_.input_mode = INPUT_MODE_VEL_RAMP;
-
-        input_pos_ = 0.0f;
-        input_pos_updated();
-        input_vel_ = -config_.homing_speed;
-        input_current_ = 0.0f;
-
-        axis_->homing_.isHomed = false;
-        axis_->homing_.homing_state = HOMING_STATE_HOMING;
-    } else {
-        return false;
-    }
-    return true;
-}

 /*
 * This anti-cogging implementation iterates through each encoder position,
@@ -88,32 +83,29 @@ bool Controller::home_axis() {
 * This holding current is added as a feedforward term in the control loop.
 */
 bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
-    if (config_.anticogging.calib_anticogging) {
-        float pos_err = input_pos_ - pos_estimate;
-        if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold &&
-            std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold) {
-            config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_;
-        }
-        if (config_.anticogging.index < 3600) {
-            config_.control_mode = CTRL_MODE_POSITION_CONTROL;
-            input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();
-            input_vel_ = 0.0f;
-            input_current_ = 0.0f;
-            input_pos_updated();
-            return false;
-        } else {
-            config_.anticogging.index = 0;
-            config_.control_mode = CTRL_MODE_POSITION_CONTROL;
-            input_pos_ = 0.0f;  // Send the motor home
-            input_vel_ = 0.0f;
-            input_current_ = 0.0f;
-            input_pos_updated();
-            anticogging_valid_ = true;
-            config_.anticogging.calib_anticogging = false;
-            return true;
-        }
+    float pos_err = input_pos_ - pos_estimate;
+    if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold &&
+        std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold) {
+        config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_current_;
+    }
+    if (config_.anticogging.index < 3600) {
+        config_.control_mode = CTRL_MODE_POSITION_CONTROL;
+        input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();
+        input_vel_ = 0.0f;
+        input_current_ = 0.0f;
+        input_pos_updated();
+        return false;
+    } else {
+        config_.anticogging.index = 0;
+        config_.control_mode = CTRL_MODE_POSITION_CONTROL;
+        input_pos_ = 0.0f;  // Send the motor home
+        input_vel_ = 0.0f;
+        input_current_ = 0.0f;
+        input_pos_updated();
+        anticogging_valid_ = true;
+        config_.anticogging.calib_anticogging = false;
+        return true;
    }
-    return false;
 }

 void Controller::update_filter_gains() {
@@ -129,10 +121,22 @@ float limitVel(const float vel_limit, const float vel_estimate, const float vel_
 }
 }  // namespace

-bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
-    // Only runs if config_.anticogging.calib_anticogging is true; non-blocking
-    anticogging_calibration(axis_->encoder_.pos_estimate_, vel_estimate);
-    float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio();
+bool Controller::update(float* current_setpoint_output) {
+    float* pos_estimate_src = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
+            ? pos_estimate_src_ : nullptr;
+    float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_)
+            ? vel_estimate_src_ : nullptr;
+
+    float anticogging_pos = 0.0f;
+    if (config_.anticogging.calib_anticogging) {
+        if (!axis_->encoder_.pos_estimate_valid_ || !axis_->encoder_.vel_estimate_valid_) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+        // non-blocking
+        anticogging_calibration(axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_);
+        anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio();
+    }

    // Update inputs
    switch (config_.input_mode) {
@@ -219,18 +223,22 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
    float vel_des = vel_setpoint_;
    if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
        float pos_err;
-        if (config_.setpoints_in_cpr) {
-            // TODO this breaks the semantics that estimates come in on the arguments.
-            // It's probably better to call a get_estimate that will arbitrate (enc vs sensorless) instead.
-            float cpr = (float)(axis_->encoder_.config_.cpr);
+        if (!pos_estimate_src) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+
+        if (pos_wrap_src_) {
+            float cpr = *pos_wrap_src_;
            // Keep pos setpoint from drifting
            pos_setpoint_ = fmodf_pos(pos_setpoint_, cpr);
            // Circular delta
-            pos_err = pos_setpoint_ - axis_->encoder_.pos_cpr_;
+            pos_err = pos_setpoint_ - *pos_estimate_src;
            pos_err = wrap_pm(pos_err, 0.5f * cpr);
        } else {
-            pos_err = pos_setpoint_ - pos_estimate;
+            pos_err = pos_setpoint_ - *pos_estimate_src;
        }
+
        vel_des += config_.pos_gain * pos_err;
        // V-shaped gain shedule based on position error
        float abs_pos_err = std::abs(pos_err);
@@ -248,7 +256,11 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s

    // Check for overspeed fault (done in this module (controller) for cohesion with vel_lim)
    if (config_.enable_overspeed_error) {  // 0.0f to disable
-        if (std::abs(vel_estimate) > config_.vel_limit_tolerance * vel_lim) {
+        if (!vel_estimate_src) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+        if (std::abs(*vel_estimate_src) > config_.vel_limit_tolerance * vel_lim) {
            set_error(ERROR_OVERSPEED);
            return false;
        }
@@ -264,17 +276,27 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
        Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast<int>(anticogging_pos), 3600), 0, 3600)];
    }

-    float v_err = vel_des - vel_estimate;
+    float v_err = 0.0f;
    if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
-        Iq += (config_.vel_gain * gain_scheduling_multiplier) * v_err;
-    }
+        if (!vel_estimate_src) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }

-    // Velocity integral action before limiting
-    Iq += vel_integrator_current_;
+        v_err = vel_des - *vel_estimate_src;
+        Iq += (config_.vel_gain * gain_scheduling_multiplier) * v_err;
+
+        // Velocity integral action before limiting
+        Iq += vel_integrator_current_;
+    }

    // Velocity limiting in current mode
    if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL && config_.enable_current_vel_limit) {
-        Iq = limitVel(config_.vel_limit, vel_estimate, config_.vel_gain, Iq);
+        if (!vel_estimate_src) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+        Iq = limitVel(config_.vel_limit, *vel_estimate_src, config_.vel_gain, Iq);
    }

    // Current limiting
@@ -14,6 +14,7 @@ public:
        ERROR_UNSTABLE_GAIN        = 0x04,
        ERROR_INVALID_MIRROR_AXIS  = 0x08,
        ERROR_INVALID_LOAD_ENCODER = 0x10,
+        ERROR_INVALID_ESTIMATE     = 0x20,
    };

    // Note: these should be sorted from lowest level of control to
@@ -54,8 +55,8 @@ public:
        float vel_gain = 5.0f / 10000.0f;  // [A/(counts/s)]
        // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
        float vel_integrator_gain    = 10.0f / 10000.0f;  // [A/(counts/s * s)]
-        float vel_limit              = 20000.0f;          // [counts/s]
-        float vel_limit_tolerance    = 1.2f;              // ratio to vel_lim. 0.0f to disable
+        float vel_limit              = 20000.0f;          // [counts/s] Infinity to disable.
+        float vel_limit_tolerance    = 1.2f;              // ratio to vel_lim. Infinity to disable.
        float vel_ramp_rate          = 10000.0f;          // [(counts/s) / s]
        float current_ramp_rate      = 1.0f;              // A / sec
        bool setpoints_in_cpr        = false;
@@ -67,11 +68,10 @@ public:
        bool enable_gain_scheduling   = false;
        bool enable_vel_limit         = true;
        bool enable_overspeed_error   = true;
-        bool enable_current_vel_limit = true;
+        bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
        uint8_t axis_to_mirror        = -1;
        float mirror_ratio            = 1.0f;
-        bool use_load_encoder         = false;
-        uint8_t load_encoder_axis     = -1;
+        uint8_t load_encoder_axis     = -1; // default depends on Axis number and is set in load_configuration()
        float load_encoder_ratio      = 1.0f;
    };

@@ -80,19 +80,18 @@ public:
    void set_error(Error_t error);

    void input_pos_updated();
+    bool select_encoder(size_t encoder_num);

    // Trajectory-Planned control
    void move_to_pos(float goal_point);
    void move_incremental(float displacement, bool from_goal_point);
-
-    bool home_axis();
    
    // TODO: make this more similar to other calibration loops
    void start_anticogging_calibration();
    bool anticogging_calibration(float pos_estimate, float vel_estimate);

    void update_filter_gains();
-    bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
+    bool update(float* current_setpoint);

    Config_t& config_;
    Axis* axis_ = nullptr; // set by Axis constructor
@@ -105,6 +104,12 @@ public:

    Error_t error_ = ERROR_NONE;

+    float* pos_estimate_src_ = nullptr;
+    bool* pos_estimate_valid_src_ = nullptr;
+    float* vel_estimate_src_ = nullptr;
+    bool* vel_estimate_valid_src_ = nullptr;
+    int32_t* pos_wrap_src_ = nullptr; // enables circular position setpoints if not null. The value pointed to is the maximum position value.
+
    float pos_setpoint_ = 0.0f;
    float vel_setpoint_ = 0.0f;
    // float vel_setpoint = 800.0f; <sensorless example>
@@ -157,7 +162,6 @@ public:
                                 make_protocol_property("inertia", &config_.inertia),
                                 make_protocol_property("axis_to_mirror", &config_.axis_to_mirror),
                                 make_protocol_property("mirror_ratio", &config_.mirror_ratio),
-                                 make_protocol_property("use_load_encoder", &config_.use_load_encoder),
                                 make_protocol_property("load_encoder_ratio", &config_.load_encoder_ratio),
                                 make_protocol_property("load_encoder_axis", &config_.load_encoder_axis),
                                 make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth,
@@ -171,8 +175,7 @@ public:
                                                      make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio),
                                                      make_protocol_property("anticogging_enabled", &config_.anticogging.enable))),
            make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"),
-            make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration),
-            make_protocol_function("home_axis", *this, &Controller::home_axis)
+            make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
        );
    }
 };
@@ -32,6 +32,8 @@ void Encoder::setup() {
 }

 void Encoder::set_error(Error_t error) {
+    vel_estimate_valid_ = false;
+    pos_estimate_valid_ = false;
    error_ |= error;
    axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
 }
@@ -525,5 +527,7 @@ bool Encoder::update() {
    // ph = fmodf(ph, 2*M_PI);
    phase_ = wrap_pm_pi(ph);

+    vel_estimate_valid_ = true;
+    pos_estimate_valid_ = true;
    return true;
 }
@@ -94,6 +94,9 @@ public:
    int32_t pos_abs_ = 0;
    float spi_error_rate_ = 0.0f;

+    bool pos_estimate_valid_ = false;
+    bool vel_estimate_valid_ = false;
+
    int16_t tim_cnt_sample_ = 0; // 
    // Updated by low_level pwm_adc_cb
    uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
@@ -2,19 +2,19 @@

 Endstop::Endstop(Endstop::Config_t& config)
    : config_(config) {
-    set_endstop_enabled(config_.enabled);
+    update_config();
 }

 void Endstop::update() {
    uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
    GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
-    auto last_pin_state = pin_state_;
+    bool last_pin_state = pin_state_;
    pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
+    float now = axis_->loop_counter_ * current_meas_period;
    if (pin_state_ != last_pin_state) {
-        debounce_timer_ = axis_->loop_counter_ * current_meas_period;
+        debounce_timer_ = now;
    }
    if (config_.enabled) {
-        float now = axis_->loop_counter_ * current_meas_period;
        if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) {         // 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 * 0.001f);              // Ensure timer doesn't have overflow issues
@@ -26,15 +26,15 @@ void Endstop::update() {
    }
 }

-bool Endstop::getEndstopState() {
+bool Endstop::get_state() {
    return endstop_state_;
 }

-void Endstop::update_endstop_config(){
-    set_endstop_enabled(config_.enabled);
+void Endstop::update_config(){
+    set_enabled(config_.enabled);
 }

-void Endstop::set_endstop_enabled(bool enable) {
+void Endstop::set_enabled(bool enable) {
    if (config_.gpio_num != 0) {
        uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
        GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
@@ -6,7 +6,7 @@ class Endstop {
    struct Config_t {
        float offset      = 0;
        float debounce_ms = 50.0f;
-        uint16_t gpio_num;
+        uint16_t gpio_num = 0;
        bool enabled        = false;
        bool is_active_high = false;
    };
@@ -16,11 +16,11 @@ class Endstop {
    Endstop::Config_t& config_;
    Axis* axis_ = nullptr;

-    void update_endstop_config();
-    void set_endstop_enabled(bool enable);
+    void update_config();
+    void set_enabled(bool enabled);

    void update();
-    bool getEndstopState();
+    bool get_state();

    bool endstop_state_ = false;

@@ -29,9 +29,9 @@ class Endstop {
            make_protocol_ro_property("endstop_state", &endstop_state_),
            make_protocol_object("config",
                                 make_protocol_property("gpio_num", &config_.gpio_num,
-                                                        [](void* ctx) { static_cast<Endstop*>(ctx)->update_endstop_config(); }, this),
+                                                        [](void* ctx) { static_cast<Endstop*>(ctx)->update_config(); }, this),
                                 make_protocol_property("enabled", &config_.enabled,
-                                                        [](void* ctx) { static_cast<Endstop*>(ctx)->update_endstop_config(); }, this),
+                                                        [](void* ctx) { static_cast<Endstop*>(ctx)->update_config(); }, this),
                                 make_protocol_property("offset", &config_.offset),
                                 make_protocol_property("is_active_high", &config_.is_active_high),
                                 make_protocol_property("debounce_ms", &config_.debounce_ms)));
@@ -85,6 +85,7 @@ extern "C" int load_configuration(void) {
            Axis::load_default_can_id(i, axis_configs[i]);
            min_endstop_configs[i] = Endstop::Config_t();
            max_endstop_configs[i] = Endstop::Config_t();
+            controller_configs[i].load_encoder_axis = i;
        }
    } else {
        user_config_loaded_ = true;
@@ -65,6 +65,7 @@ bool SensorlessEstimator::update() {
    // Check that we don't get problems with discrete time approximation
    if (!(current_meas_period * pll_kp < 1.0f)) {
        error_ |= ERROR_UNSTABLE_GAIN;
+        vel_estimate_valid_ = false;
        return false;
    }

@@ -77,5 +78,6 @@ bool SensorlessEstimator::update() {
    // update PLL velocity
    vel_estimate_ += current_meas_period * pll_ki * delta_phase;

+    vel_estimate_valid_ = true;
    return true;
 };
@@ -26,6 +26,7 @@ public:
    float phase_ = 0.0f;                        // [rad]
    float pll_pos_ = 0.0f;                      // [rad]
    float vel_estimate_ = 0.0f;                      // [rad/s]
+    bool vel_estimate_valid_ = false;
    // float pll_kp_ = 0.0f;                       // [rad/s / rad]
    // float pll_ki_ = 0.0f;                       // [(rad/s^2) / rad]
    float flux_state_[2] = {0.0f, 0.0f};        // [Vs]