diff --git a/CHANGELOG.md b/CHANGELOG.md
index bd3a75c2..8dc021f7 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -5,6 +5,8 @@ Please add a note of your changes below this heading if you make a Pull Request.
 * Ensure endstops update before being checked for errors, to prevent [#625](https://github.com/odriverobotics/ODrive/issues/625)
 * Reset trajectory_done_ during homing to ensure a new trajectory is actually computed [#634](https://github.com/odriverobotics/ODrive/issues/634)
 * Use `input_xxx` as a DC offset in tuning mode
+* Sync `steps_` with input pos.
+* Trigger reset of input_pos and pos_setpoint to estimate when changing control mode into position control
 
 # Releases
 ## [0.5.4] - 2021-10-12
diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index d8579c5b..f11a9878 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -287,19 +287,7 @@ bool Axis::start_closed_loop_control() {
         }
 
         // To avoid any transient on startup, we intialize the setpoint to be the current position
-        if (controller_.config_.control_mode >= Controller::CONTROL_MODE_POSITION_CONTROL) {
-            std::optional<float> pos_init = (controller_.config_.circular_setpoints ?
-                                    controller_.pos_estimate_circular_src_ :
-                                    controller_.pos_estimate_linear_src_).any();
-            if (!pos_init.has_value()) {
-                return false;
-            } else {
-                controller_.pos_setpoint_ = *pos_init;
-                controller_.input_pos_ = *pos_init;
-                float range = controller_.config_.circular_setpoint_range;
-                steps_ = (int64_t)(fmodf_pos(*pos_init, range) / range * controller_.config_.steps_per_circular_range);
-            }
-        }
+        controller_.control_mode_updated();
         controller_.input_pos_updated();
 
         // Avoid integrator windup issues
diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index e17e8629..4cf2700c 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -87,6 +87,32 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate)
     }
 }
 
+void Controller::set_input_pos_and_steps(float const pos) {
+    input_pos_ = pos;
+    if (config_.circular_setpoints) {
+        float const range = config_.circular_setpoint_range;
+        axis_->steps_ = (int64_t)(fmodf_pos(pos, range) / range * config_.steps_per_circular_range);
+    } else {
+        axis_->steps_ = (int64_t)(pos * config_.steps_per_circular_range);
+    }
+}
+
+bool Controller::control_mode_updated() {
+    if (config_.control_mode >= CONTROL_MODE_POSITION_CONTROL) {
+        std::optional<float> estimate = (config_.circular_setpoints ?
+                                pos_estimate_circular_src_ :
+                                pos_estimate_linear_src_).any();
+        if (!estimate.has_value()) {
+            return false;
+        }
+
+        pos_setpoint_ = *estimate;
+        set_input_pos_and_steps(*estimate);
+    }
+    return true;
+}
+
+
 void Controller::update_filter_gains() {
     float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz);
     input_filter_ki_ = 2.0f * bandwidth;  // basic conversion to discrete time
@@ -109,11 +135,15 @@ bool Controller::update() {
     std::optional<float> anticogging_vel_estimate = axis_->encoder_.vel_estimate_.present();
 
     if (axis_->step_dir_active_) {
-        if (!pos_wrap.has_value()) {
-            set_error(ERROR_INVALID_CIRCULAR_RANGE);
-            return false;
+        if (config_.circular_setpoints) {
+            if (!pos_wrap.has_value()) {
+                set_error(ERROR_INVALID_CIRCULAR_RANGE);
+                return false;
+            }
+            input_pos_ = (float)(axis_->steps_ % config_.steps_per_circular_range) * (*pos_wrap / (float)(config_.steps_per_circular_range));
+        } else {
+            input_pos_ = (float)(axis_->steps_) / (float)(config_.steps_per_circular_range);
         }
-        input_pos_ = (float)(axis_->steps_ % config_.steps_per_circular_range) * (*pos_wrap / (float)(config_.steps_per_circular_range));
     }
 
     if (config_.anticogging.calib_anticogging) {
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index da19149e..2fc54861 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -58,6 +58,7 @@ public:
         Controller* parent;
         void set_input_filter_bandwidth(float value) { input_filter_bandwidth = value; parent->update_filter_gains(); }
         void set_steps_per_circular_range(uint32_t value) { steps_per_circular_range = value > 0 ? value : steps_per_circular_range; }
+        void set_control_mode(ControlMode value) { control_mode = value; parent->control_mode_updated(); }
     };
 
     
@@ -69,6 +70,8 @@ public:
     constexpr void input_pos_updated() {
         input_pos_updated_ = true;
     }
+    bool control_mode_updated();
+    void set_input_pos_and_steps(float pos);
 
     bool select_encoder(size_t encoder_num);
 
@@ -122,7 +125,7 @@ public:
     OutputPort<float> torque_output_ = 0.0f;
 
     // custom setters
-    void set_input_pos(float value) { input_pos_ = value; input_pos_updated(); }
+    void set_input_pos(float value) { set_input_pos_and_steps(value); input_pos_updated(); }
 };
 
 #endif // __CONTROLLER_HPP
diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp
index 961d4c82..93b9f7bc 100644
--- a/Firmware/communication/can/can_simple.cpp
+++ b/Firmware/communication/can/can_simple.cpp
@@ -253,7 +253,7 @@ bool CANSimple::get_encoder_count_callback(const Axis& axis) {
 }
 
 void CANSimple::set_input_pos_callback(Axis& axis, const can_Message_t& msg) {
-    axis.controller_.input_pos_ = can_getSignal<float>(msg, 0, 32, true);
+    axis.controller_.set_input_pos_and_steps(can_getSignal<float>(msg, 0, 32, true));
     axis.controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.001f, 0);
     axis.controller_.input_torque_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.001f, 0);
     axis.controller_.input_pos_updated();
@@ -269,8 +269,10 @@ void CANSimple::set_input_torque_callback(Axis& axis, const can_Message_t& msg)
 }
 
 void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& msg) {
-    axis.controller_.config_.control_mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
+    Controller::ControlMode const mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
+    axis.controller_.config_.control_mode = static_cast<Controller::ControlMode>(mode);
     axis.controller_.config_.input_mode = static_cast<Controller::InputMode>(can_getSignal<int32_t>(msg, 32, 32, true));
+    axis.controller_.control_mode_updated();
 }
 
 void CANSimple::set_limits_callback(Axis& axis, const can_Message_t& msg) {
diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml
index 2f9da82b..77b997fe 100644
--- a/Firmware/odrive-interface.yaml
+++ b/Firmware/odrive-interface.yaml
@@ -1013,6 +1013,7 @@ interfaces:
         c_setter: set_input_pos
         doc: |
           Set the desired position of the axis.  Only valid in `CONTROL_MODE_POSITION_CONTROL`.startup.
+          Also updates the step count.
           In `INPUT_MODE_TUNING`, this acts as a DC offset for the position sine wave.
       input_vel:
         type: float32
@@ -1068,7 +1069,7 @@ interfaces:
           enable_overspeed_error: 
             type: bool
             doc: Enables the velocity controller's overspeed error
-          control_mode: ControlMode
+          control_mode: {type: ControlMode, c_setter: set_control_mode}
           input_mode: InputMode
           pos_gain:
             type: float32