add option to use velocity setpoint feedforward

Firmware/MotorControl/axis.cpp

@@ -207,7 +207,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme
         motor_.current_control_.phase_src_.connect_to(&open_loop_controller_.phase_);
         acim_estimator_.rotor_phase_src_.connect_to(&open_loop_controller_.phase_);
         
-        motor_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_);
+        motor_.vel_src_.disconnect();
         motor_.current_control_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_);
         acim_estimator_.rotor_phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_);
     }
@@ -318,9 +318,14 @@ bool Axis::start_closed_loop_control() {
         acim_estimator_.rotor_phase_src_.connect_to(phase_src);
         
         OutputPort<float>* phase_vel_src = sensorless_mode ? &sensorless_estimator_.phase_vel_ : &encoder_.phase_vel_;
-        motor_.phase_vel_src_.connect_to(phase_vel_src);
         motor_.current_control_.phase_vel_src_.connect_to(phase_vel_src);
         acim_estimator_.rotor_phase_vel_src_.connect_to(phase_vel_src);
+
+        if (motor_.config_.vel_setpoint_FF) {
+            motor_.vel_src_.connect_to(&controller_.vel_setpoint_);
+        } else {
+            motor_.vel_src_.connect_to(sensorless_mode ? &sensorless_estimator_.vel_estimate_ : &encoder_.vel_estimate_);
+        }
         
         if (sensorless_mode) {
             // Make the final velocity of the loĉk-in spin the setpoint of the

Firmware/MotorControl/encoder.cpp

@@ -364,7 +364,7 @@ bool Encoder::run_offset_calibration() {
         axis_->motor_.current_control_.phase_src_.connect_to(&axis_->open_loop_controller_.phase_);
         axis_->acim_estimator_.rotor_phase_src_.connect_to(&axis_->open_loop_controller_.phase_);
 
-        axis_->motor_.phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_);
+        axis_->motor_.vel_src_.disconnect();
         axis_->motor_.current_control_.phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_);
         axis_->acim_estimator_.rotor_phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_);
     }

Firmware/MotorControl/motor.cpp

@@ -555,27 +555,31 @@ void Motor::update(uint32_t timestamp) {
     float vd = 0.0f;
     float vq = 0.0f;
 
-    std::optional<float> phase_vel = phase_vel_src_.present();
+    std::optional<float> vel = vel_src_.present();
 
     if (config_.R_wL_FF_enable) {
-        if (!phase_vel.has_value()) {
-            error_ |= ERROR_UNKNOWN_PHASE_VEL;
+        if (!vel.has_value()) {
+            error_ |= ERROR_UNKNOWN_VEL;
             return;
         }
 
-        vd -= *phase_vel * config_.phase_inductance * iq;
-        vq += *phase_vel * config_.phase_inductance * id;
+        float phase_vel = (2*M_PI) * (*vel) * config_.pole_pairs * direction_;
+
+        vd -= phase_vel * config_.phase_inductance * iq;
+        vq += phase_vel * config_.phase_inductance * id;
         vd += config_.phase_resistance * id;
         vq += config_.phase_resistance * iq;
     }
 
     if (config_.bEMF_FF_enable) {
-        if (!phase_vel.has_value()) {
-            error_ |= ERROR_UNKNOWN_PHASE_VEL;
+        if (!vel.has_value()) {
+            error_ |= ERROR_UNKNOWN_VEL;
             return;
         }
 
-        vq += *phase_vel * (2.0f/3.0f) * (config_.torque_constant / config_.pole_pairs);
+        float phase_vel = (2*M_PI) * (*vel) * config_.pole_pairs * direction_;
+
+        vq += phase_vel * (2.0f/3.0f) * (config_.torque_constant / config_.pole_pairs);
     }
     
     if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {

Firmware/MotorControl/motor.hpp

@@ -42,6 +42,7 @@ public:
         
         bool R_wL_FF_enable = false; // Enable feedforwards for R*I and w*L*I terms
         bool bEMF_FF_enable = false; // Enable feedforward for bEMF
+        bool vel_setpoint_FF = false;
 
         float I_bus_hard_min = -INFINITY;
         float I_bus_hard_max = INFINITY;
@@ -125,7 +126,7 @@ public:
     float max_dc_calib_ = 0.0f; // [A] set in setup()
 
     InputPort<float> torque_setpoint_src_; // Usually points to the Controller object's output
-    InputPort<float> phase_vel_src_; // Usually points to the Encoder object's output
+    InputPort<float> vel_src_; // Usually points to the Encoder object's output
 
     float direction_ = 0.0f; // if -1 then positive torque is converted to negative Iq
     OutputPort<float2D> Vdq_setpoint_ = {{0.0f, 0.0f}}; // fed to the FOC

Firmware/odrive-interface.yaml

@@ -651,7 +651,7 @@ interfaces:
               See `ODrive.Error` for more details.
           BAD_TIMING: {doc: The main control loop got out of sync with the motor control loop. This could indicate that the main control loop got stuck.}
           UNKNOWN_PHASE_ESTIMATE: {doc: The current controller did not get a valid angle input. Maybe you didn't calibrate the encoder.}
-          UNKNOWN_PHASE_VEL: {doc: The motor controller did not get a valid phase velocity input.}
+          UNKNOWN_VEL: {doc: The motor controller did not get a valid velocity input.}
           UNKNOWN_TORQUE: {doc: The motor controller did not get a valid torque input.}
           UNKNOWN_CURRENT_COMMAND: {doc: The current controller did not get a valid current setpoint. Maybe you didn't configure the controller correctly.}
           UNKNOWN_CURRENT_MEASUREMENT: {doc: The current controller did not get a valid current measurement.}
@@ -729,6 +729,13 @@ interfaces:
           acim_autoflux_decay_gain: float32
           R_wL_FF_enable: bool
           bEMF_FF_enable: bool
+          vel_setpoint_FF:
+            type: bool
+            doc: |
+              If true, the feedforward terms that are enabled by
+              `R_wL_FF_enable` and `bEMF_FF_enable` are calculated from the
+              controller's velocity setpoint rather than the velocity estimate.
+              Default: false.
           I_bus_hard_min:
             type: float32
             unit: A

tools/odrive/enums.py

@@ -115,7 +115,7 @@ MOTOR_ERROR_BRAKE_RESISTOR_DISARMED      = 0x00800000
 MOTOR_ERROR_SYSTEM_LEVEL                 = 0x01000000
 MOTOR_ERROR_BAD_TIMING                   = 0x02000000
 MOTOR_ERROR_UNKNOWN_PHASE_ESTIMATE       = 0x04000000
-MOTOR_ERROR_UNKNOWN_PHASE_VEL            = 0x08000000
+MOTOR_ERROR_UNKNOWN_VEL                  = 0x08000000
 MOTOR_ERROR_UNKNOWN_TORQUE               = 0x10000000
 MOTOR_ERROR_UNKNOWN_CURRENT_COMMAND      = 0x20000000
 MOTOR_ERROR_UNKNOWN_CURRENT_MEASUREMENT  = 0x40000000