diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 2afb00b3..f5e64724 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -315,19 +315,25 @@ bool Axis::run_closed_loop_control_loop() {
             return false; // set_error should update axis.error_
 
         // Handle the homing case
-        if (homing_state_ == HOMING_STATE_HOMING) {
+        if (homing_.homing_state == HOMING_STATE_HOMING) {
             if (min_endstop_.getEndstopState()) {
                 encoder_.set_linear_count(min_endstop_.config_.offset);
+
+                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;
-                controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
-                controller_.input_pos_updated();
-                homing_state_ = HOMING_STATE_MOVE_TO_ZERO;
+
+                homing_.homing_state = HOMING_STATE_MOVE_TO_ZERO;
             }
-        } else if (homing_state_ == HOMING_STATE_MOVE_TO_ZERO) {
-            if(!min_endstop_.getEndstopState()){
-                homing_state_ = HOMING_STATE_IDLE;
+        } 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;
             }
         } else {
             // Check for endstop presses
diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index 776e4e02..f53ae0c1 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -90,6 +90,12 @@ public:
         uint32_t can_heartbeat_rate_ms = 100;
     };
 
+    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;
+    };
+
     enum thread_signals {
         M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
     };
@@ -250,7 +256,7 @@ public:
     State_t& current_state_ = task_chain_[0];
     uint32_t loop_counter_ = 0;
     LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
-    HomingState_t homing_state_ = HOMING_STATE_IDLE;
+    Homing_t homing_;
     uint32_t last_heartbeat_ = 0;
 
     // watchdog
@@ -265,7 +271,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_state_),
+            make_protocol_ro_property("homing_state", &homing_.homing_state),
             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),
diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 0080c789..b77d6ed1 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -61,12 +61,18 @@ void Controller::start_anticogging_calibration() {
 //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_state_ = HOMING_STATE_HOMING;
+
+        axis_->homing_.homing_state = HOMING_STATE_HOMING;
     } else {
         return false;
     }