implement circular vel pll tracking

Firmware/MotorControl/encoder.cpp

@@ -50,11 +50,20 @@ void Encoder::set_count(int32_t count) {
     // Disable interrupts to make a critical section to avoid race condition
     uint32_t prim = __get_PRIMASK();
     __disable_irq();
+
+    // Update states
+    shadow_count_ = count;
+    pos_estimate_ = (float)count;
+    count_in_cpr_ = mod(count, config_.cpr);
+    pos_cpr = (float)count_in_cpr_;
+
     // Offset and state must be shifted by the same amount
     offset_ += count - shadow_count_;
-    shadow_count_ = count; //TODO FIXME
+    offset_ = mod(offset_, config_.cpr);
+
+    //Write hardware last
     hw_config_.timer->Instance->CNT = count;
-    pll_pos_ = (float)count;
+
     __set_PRIMASK(prim);
 }
 
@@ -215,16 +224,19 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp
     // run pll (for now pll is in units of encoder counts)
     // Predict current pos
     pos_estimate_ += current_meas_period * pll_vel_;
-    pll_pos_ += current_meas_period * pll_vel_;
+    pos_cpr       += current_meas_period * pll_vel_;
     // discrete phase detector
-    // float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_));
-    float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_));
+    float delta_pos     = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_));
+    float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr));
+    delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
     // pll feedback
-    pll_pos_ += current_meas_period * pll_kp_ * delta_pos;
-    pll_vel_ += current_meas_period * pll_ki_ * delta_pos;
+    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;
 
     // Assign output arguments
-    if (pos_estimate) *pos_estimate = pll_pos_;
+    if (pos_estimate) *pos_estimate = pos_estimate_;
     if (vel_estimate) *vel_estimate = pll_vel_;
     if (phase_output) *phase_output = phase_;
     return true;

Firmware/MotorControl/encoder.hpp

@@ -55,7 +55,7 @@ public:
     int32_t offset_ = 0;
     float phase_ = 0.0f;    // [rad]
     float pos_estimate_ = 0.0f;  // [rad]
-    float pll_pos_ = 0.0f;  // [rad]
+    float pos_cpr = 0.0f;  // [rad]
     float pll_vel_ = 0.0f;  // [rad/s]
     float pll_kp_ = 0.0f;   // [rad/s / rad]
     float pll_ki_ = 0.0f;   // [(rad/s^2) / rad]
@@ -71,7 +71,7 @@ public:
             make_protocol_property("offset", &offset_),
             make_protocol_property("phase", &phase_),
             make_protocol_property("pos_estimate", &pos_estimate_),
-            make_protocol_property("pll_pos", &pll_pos_),
+            make_protocol_property("pos_cpr", &pos_cpr),
             make_protocol_property("pll_vel", &pll_vel_),
             make_protocol_property("pll_kp", &pll_kp_),
             make_protocol_property("pll_ki", &pll_ki_),

Firmware/MotorControl/utils.h

@@ -7,6 +7,7 @@ extern "C" {
 #endif
 
 #include <stdint.h>
+#include <math.h>
 
 /**
  * @brief Unique ID register address location
@@ -94,6 +95,14 @@ static inline float wrap_pm_pi(float theta) {
     return wrap_pm(theta, M_PI);
 }
 
+// like fmodf, but always positive
+static inline float fmodf_pos(float x, float y) {
+    float out = fmodf(x, y);
+    if (out < 0.0f)
+        out += y;
+    return out;
+}
+
 // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
 // as per the magnitude invariant clarke transform
 // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2

tools/odrive/shell.py

@@ -22,7 +22,7 @@ def print_help(args):
         print('Type "odrv0." and press <tab>')
     print('This will present you with all the properties that you can reference')
     print('')
-    print('For example: "odrv0.motor0.encoder.pll_pos"')
+    print('For example: "odrv0.motor0.encoder.pos_estimate"')
     print('will print the current encoder position on motor 0')
     print('and "odrv0.motor0.pos_setpoint = 10000"')
     print('will send motor0 to 10000')

tools/odrive/tests.py

@@ -342,13 +342,13 @@ class TestClosedLoopControl(AxisTest):
         time.sleep(0.001)
         test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL)
         time.sleep(0.1) # give the PLL some time to settle
-        init_pos = axis_ctx.handle.encoder.pll_pos
+        init_pos = axis_ctx.handle.encoder.pos_estimate
         axis_ctx.handle.controller.set_pos_setpoint(init_pos+1000, 0, 0)
         time.sleep(0.5)
-        test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos+1000, range=200)
+        test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos+1000, range=200)
         axis_ctx.handle.controller.set_pos_setpoint(init_pos-1000, 0, 0)
         time.sleep(0.5)
-        test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos-1000, range=400)
+        test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos-1000, range=400)
 
         logger.debug("closed loop control: test vel_limit")
         axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0)
@@ -551,7 +551,7 @@ class TestVelCtrlVsPosCtrl(DualAxisTest):
         # Turn to some position
         logger.debug("using {} as driver against load, vel=100000...".format(driver_ctx.name))
         set_limits(driver_ctx, logger, vel_limit=100000, current_limit=50)
-        init_pos = driver_ctx.handle.encoder.pll_pos
+        init_pos = driver_ctx.handle.encoder.pos_estimate
         driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
         request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
         for _ in range(int(4000/5)):
@@ -563,7 +563,7 @@ class TestVelCtrlVsPosCtrl(DualAxisTest):
 
         logger.debug("using {} as driver against load, vel=20000...".format(driver_ctx.name))
         set_limits(driver_ctx, logger, vel_limit=20000, current_limit=50)
-        init_pos = driver_ctx.handle.encoder.pll_pos
+        init_pos = driver_ctx.handle.encoder.pos_estimate
         driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
         request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)
         #for _ in range(int(5*4000/5)):
@@ -580,6 +580,6 @@ class TestVelCtrlVsPosCtrl(DualAxisTest):
         ## Turn to another position
         #logger.debug("controlling against load, vel=40000...")
         #set_limits(axis1_ctx, logger, vel_limit=40000, current_limit=20)
-        #init_pos = axis1_ctx.handle.encoder.pll_pos
+        #init_pos = axis1_ctx.handle.encoder.pos_estimate
         #axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0)
         #request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL)

tools/odrivetool

@@ -100,8 +100,8 @@ try:
 
         # If you want to plot different values, change them here.
         # You can plot any number of values concurrently.
-        start_liveplotter(lambda: [my_odrive.motor0.encoder.pll_pos,
-                                my_odrive.motor1.encoder.pll_pos])
+        start_liveplotter(lambda: [my_odrive.motor0.encoder.pos_estimate,
+                                my_odrive.motor1.encoder.pos_estimate])
 
     elif args.command == 'drv-status':
         from odrive.utils import print_drv_regs