Sets steps_ in CANSimple::set_input_pos_callback

... because otherwise, the effect of the callback is canceled by an
assignment in the main update() loop, that calculates input_pos
based on steps_, if we're in step/dir mode.

- Fixes a steps_ underflow/wrapping bug

CHANGELOG.md

@@ -5,6 +5,7 @@ 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.
 
 # Releases
 ## [0.5.4] - 2021-10-12

Firmware/MotorControl/axis.cpp

@@ -295,9 +295,7 @@ bool Axis::start_closed_loop_control() {
                 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_.set_input_pos_and_steps(*pos_init);
             }
         }
         controller_.input_pos_updated();

Firmware/MotorControl/controller.cpp

@@ -87,6 +87,12 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate)
     }
 }
 
+void Controller::set_input_pos_and_steps(float const pos) {
+    input_pos_ = pos;
+    float const range = config_.circular_setpoint_range;
+    axis_->steps_ = (int64_t)(wrap_pm(pos, range) / range * config_.steps_per_circular_range);
+}
+
 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

Firmware/MotorControl/controller.hpp

@@ -80,6 +80,8 @@ public:
     void start_anticogging_calibration();
     bool anticogging_calibration(float pos_estimate, float vel_estimate);
 
+    void set_input_pos_and_steps(float pos);
+
     void update_filter_gains();
     bool update();
 

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();