Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-16 01:49:06 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #224 from Wetmelon/rem_pll

Browse Source 
Rename pll_vel to vel_estimate
This commit is contained in:
Oskar Weigl
2018-08-24 15:41:44 -07:00
committed by GitHub
parent cf9cf854a4 14e917171d
commit bcae4bc7a8
6 changed files with 27 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+2 -2
View File
@@ -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_, &current_setpoint))
if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_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_, &current_setpoint))
if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_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_
Firmware/MotorControl/encoder.cpp
+7 -7
View File
@@ -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;
Firmware/MotorControl/encoder.hpp
+2 -2
View File
@@ -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",
Firmware/MotorControl/sensorless_estimator.cpp
+2 -2
View File
@@ -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;
};
Firmware/MotorControl/sensorless_estimator.hpp
+2 -2
View File
@@ -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",
tools/odrive/tests.py
+12 -12
View File
@@ -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)