Makes behavior consistent across interfaces

Defines and uses set_setpoint_to_estimate() reusable function
Uses it in three places:
 - axis.cpp to avoid transient on startup
 - can_simple.cpp to avoid sudden movement upon mode change
 - odrive-interface.yaml just to make it executable via USB and UART

Swaps set_input_pos() to set_input_pos_and_steps() so that we get the
same behavior when changing input_pos over USB and UART as we get when
we change it via CAN.

Firmware/MotorControl/axis.cpp

@@ -288,15 +288,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_.set_input_pos_and_steps(*pos_init);
-            }
+            controller_.set_setpoint_to_estimate();
         }
         controller_.input_pos_updated();
 

Firmware/MotorControl/controller.cpp

@@ -97,6 +97,19 @@ void Controller::set_input_pos_and_steps(float const pos) {
     }
 }
 
+bool Controller::set_setpoint_to_estimate() {
+    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);

Firmware/MotorControl/controller.hpp

@@ -81,6 +81,7 @@ class Controller : public ODriveIntf::ControllerIntf {
     bool anticogging_calibration(float pos_estimate, float vel_estimate);
 
     void set_input_pos_and_steps(float pos);
+    bool set_setpoint_to_estimate();
 
     void update_filter_gains();
     bool update();
@@ -124,7 +125,7 @@ class Controller : public ODriveIntf::ControllerIntf {
     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

Firmware/communication/can/can_simple.cpp

@@ -272,14 +272,7 @@ void CANSimple::set_controller_modes_callback(Axis& axis, const can_Message_t& m
 
     Controller::ControlMode const mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
     if (mode == Controller::CONTROL_MODE_POSITION_CONTROL) {
-        float estimate = 0.0f;
-        if (axis.controller_.config_.circular_setpoints) {
-            estimate = axis.encoder_.pos_circular_.any().value_or(0.0f);
-        } else {
-            estimate = axis.encoder_.pos_estimate_.any().value_or(0.0f);
-        }
-
-        axis.controller_.set_input_pos_and_steps(estimate);
+        axis.controller_.set_setpoint_to_estimate();
     }
 
     axis.controller_.config_.control_mode = static_cast<Controller::ControlMode>(mode);

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
@@ -1610,6 +1611,7 @@ valuetypes:
         doc: |
           Uses the inner torque loop, the velocity control loop, and the outer position control loop.
           Use `input_pos` to command desired position, `input_vel` to command velocity feed-forward, and `input_torque` for torque feed-forward.
+        c_setter: set_setpoint_to_estimate
 
   ODrive.Controller.InputMode:
     values: