diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index b5c7fbe5..7d79b3d6 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -184,7 +184,7 @@ bool Axis::run_sensorless_control_loop() { // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; - if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.pll_vel_, ¤t_setpoint)) + if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, sensorless_estimator_.phase_)) return false; // set_error should update axis.error_ @@ -199,7 +199,7 @@ bool Axis::run_closed_loop_control_loop() { run_control_loop([this](){ // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; - if (!controller_.update(encoder_.pos_estimate_, encoder_.pll_vel_, ¤t_setpoint)) + if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error if (!motor_.update(current_setpoint, encoder_.phase_)) return false; // set_error should update axis.error_ diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index ccd6ee2d..48d99eee 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -288,8 +288,8 @@ bool Encoder::update() { //// run pll (for now pll is in units of encoder counts) // Predict current pos - pos_estimate_ += current_meas_period * pll_vel_; - pos_cpr_ += current_meas_period * pll_vel_; + pos_estimate_ += current_meas_period * vel_estimate_; + pos_cpr_ += current_meas_period * vel_estimate_; // discrete phase detector float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_)); float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_)); @@ -298,10 +298,10 @@ bool Encoder::update() { pos_estimate_ += current_meas_period * pll_kp * delta_pos; pos_cpr_ += current_meas_period * pll_kp * delta_pos_cpr; pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr)); - pll_vel_ += current_meas_period * pll_ki * delta_pos_cpr; + vel_estimate_ += current_meas_period * pll_ki * delta_pos_cpr; bool snap_to_zero_vel = false; - if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki) { - pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter + if (fabsf(vel_estimate_) < 0.5f * current_meas_period * pll_ki) { + vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter snap_to_zero_vel = true; } @@ -316,8 +316,8 @@ bool Encoder::update() { } else if (delta_enc < 0) { interpolation_ = 1.0f; } else { - // Interpolate (predict) between encoder counts using pll_vel, - interpolation_ += current_meas_period * pll_vel_; + // Interpolate (predict) between encoder counts using vel_estimate, + interpolation_ += current_meas_period * vel_estimate_; // don't allow interpolation indicated position outside of [enc, enc+1) if (interpolation_ > 1.0f) interpolation_ = 1.0f; if (interpolation_ < 0.0f) interpolation_ = 0.0f; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 531bbf7b..1b3d2988 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -69,7 +69,7 @@ public: float phase_ = 0.0f; // [rad] float pos_estimate_ = 0.0f; // [rad] float pos_cpr_ = 0.0f; // [rad] - float pll_vel_ = 0.0f; // [rad/s] + float vel_estimate_ = 0.0f; // [rad/s] // float pll_kp_ = 0.0f; // [rad/s / rad] // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] @@ -89,7 +89,7 @@ public: make_protocol_property("pos_estimate", &pos_estimate_), make_protocol_property("pos_cpr", &pos_cpr_), make_protocol_property("hall_state", &hall_state_), - make_protocol_property("pll_vel", &pll_vel_), + make_protocol_property("vel_estimate", &vel_estimate_), // make_protocol_property("pll_kp", &pll_kp_), // make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 25210f98..43191ce3 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -69,13 +69,13 @@ bool SensorlessEstimator::update() { } // predict PLL phase with velocity - pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_vel_); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_); // update PLL phase with observer permanent magnet phase phase_ = fast_atan2(eta[1], eta[0]); float delta_phase = wrap_pm_pi(phase_ - pll_pos_); pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase); // update PLL velocity - pll_vel_ += current_meas_period * pll_ki * delta_phase; + vel_estimate_ += current_meas_period * pll_ki * delta_phase; return true; }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 77f50943..6d15820f 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -25,7 +25,7 @@ public: Error_t error_ = ERROR_NONE; float phase_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] - float pll_vel_ = 0.0f; // [rad/s] + float vel_estimate_ = 0.0f; // [rad/s] // float pll_kp_ = 0.0f; // [rad/s / rad] // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] float flux_state_[2] = {0.0f, 0.0f}; // [Vs] @@ -38,7 +38,7 @@ public: make_protocol_property("error", &error_), make_protocol_property("phase", &phase_), make_protocol_property("pll_pos", &pll_pos_), - make_protocol_property("pll_vel", &pll_vel_), + make_protocol_property("vel_estimate", &vel_estimate_), // make_protocol_property("pll_kp", &pll_kp_), // make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 92dfc9b8..eeb5dcec 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -195,10 +195,10 @@ class AxisTest(ABC): def check_preconditions(self, axis_ctx: AxisTestContext, logger): test_assert_no_error(axis_ctx) test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) - if (abs(axis_ctx.handle.encoder.pll_vel) > 100): + if (abs(axis_ctx.handle.encoder.vel_estimate) > 100): logger.warn("axis still in motion, delaying 2 sec...") time.sleep(2) - test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500) + test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=500) test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.85, accuracy=0.001) test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.96, accuracy=0.001) @@ -218,11 +218,11 @@ class DualAxisTest(ABC): test_assert_no_error(axis1_ctx) test_assert_eq(axis0_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_eq(axis1_ctx.handle.current_state, AXIS_STATE_IDLE) - if (abs(axis0_ctx.handle.encoder.pll_vel) > 100) or (abs(axis1_ctx.handle.encoder.pll_vel) > 100): + if (abs(axis0_ctx.handle.encoder.vel_estimate) > 100) or (abs(axis1_ctx.handle.encoder.vel_estimate) > 100): logger.warn("some axis still in motion, delaying 2 sec...") time.sleep(2) - test_assert_eq(axis0_ctx.handle.encoder.pll_vel, 0, range=500) - test_assert_eq(axis1_ctx.handle.encoder.pll_vel, 0, range=500) + test_assert_eq(axis0_ctx.handle.encoder.vel_estimate, 0, range=500) + test_assert_eq(axis1_ctx.handle.encoder.vel_estimate, 0, range=500) @abc.abstractmethod def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): @@ -390,11 +390,11 @@ class TestClosedLoopControl(AxisTest): axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0) axis_ctx.handle.controller.config.vel_limit = 40000 time.sleep(0.3) - test_assert_eq(axis_ctx.handle.encoder.pll_vel, 40000, range=4000) + test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 40000, range=4000) expected_sensorless_estimation = 40000 * 2 * math.pi / axis_ctx.yaml['encoder-cpr'] * axis_ctx.yaml['motor-pole-pairs'] - test_assert_eq(axis_ctx.handle.sensorless_estimator.pll_vel, expected_sensorless_estimation, range=50) + test_assert_eq(axis_ctx.handle.sensorless_estimator.vel_estimate, expected_sensorless_estimation, range=50) time.sleep(3) - test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=1000) + test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=1000) time.sleep(0.5) request_state(axis_ctx, AXIS_STATE_IDLE) @@ -494,7 +494,7 @@ class TestHighVelocity(AxisTest): # set and measure velocity axis_ctx.handle.controller.set_vel_setpoint(vel_setpoint, 0) - measured_vel = axis_ctx.handle.encoder.pll_vel + measured_vel = axis_ctx.handle.encoder.vel_estimate max_measured_vel = max(measured_vel, max_measured_vel) test_assert_eq(measured_vel, expected_velocity, range=vel_range) test_assert_no_error(axis_ctx) @@ -512,10 +512,10 @@ class TestHighVelocity(AxisTest): axis_ctx.handle.controller.set_vel_setpoint(0, 0) time.sleep(0.5) # If the velocity integrator at work, it may now work against slowing down. - test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=rated_limit*0.3) + test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=rated_limit*0.3) # TODO: this is not a good bound, but the encoder float resolution results in a bad velocity estimate after this many turns time.sleep(0.5) - test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=2000) + test_assert_eq(axis_ctx.handle.encoder.vel_estimate, 0, range=2000) request_state(axis_ctx, AXIS_STATE_IDLE) test_assert_no_error(axis_ctx) @@ -775,6 +775,6 @@ class TestSensorlessControl(AxisTest): request_state(axis_ctx, AXIS_STATE_SENSORLESS_CONTROL) # wait for spinup time.sleep(2) - test_assert_eq(odrv0.axis0.encoder.pll_vel, target_vel, range=2000) + test_assert_eq(odrv0.axis0.encoder.vel_estimate, target_vel, range=2000) request_state(axis_ctx, AXIS_STATE_IDLE)