make input mode change trigger consistent with all sources

CHANGELOG.md

@@ -5,7 +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
-* Syncs `steps_` with input pos when set explicitly via CAN.
+* 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

Firmware/MotorControl/axis.cpp

@@ -287,9 +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) {
-            controller_.set_setpoint_to_estimate();
-        }
+        controller_.control_mode_updated();
         controller_.input_pos_updated();
 
         // Avoid integrator windup issues

Firmware/MotorControl/controller.cpp

@@ -97,16 +97,18 @@ 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;
-    }
+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);
+        pos_setpoint_ = *estimate;
+        set_input_pos_and_steps(*estimate);
+    }
     return true;
 }
 

Firmware/MotorControl/controller.hpp

@@ -69,6 +69,8 @@ class Controller : public ODriveIntf::ControllerIntf {
     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);
 
@@ -80,9 +82,6 @@ class Controller : public ODriveIntf::ControllerIntf {
     void start_anticogging_calibration();
     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();
 

Firmware/communication/can/can_simple.cpp

@@ -269,14 +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) {
-
     Controller::ControlMode const mode = static_cast<Controller::ControlMode>(can_getSignal<int32_t>(msg, 0, 32, true));
-    if (mode == Controller::CONTROL_MODE_POSITION_CONTROL) {
-        axis.controller_.set_setpoint_to_estimate();
-    }
-
     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) {

Firmware/odrive-interface.yaml

@@ -1591,6 +1591,7 @@ valuetypes:
         doc: MagAlpha MA732 magnetic encoder
 
   ODrive.Controller.ControlMode:
+    c_setter: set_setpoint_to_estimate
     values:
       # Note: these should be sorted from lowest level of control to
       # highest level of control, to allow "<" style comparisons.
@@ -1611,7 +1612,6 @@ 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: