From 8068c18aaacf397a0f39989ebd81f6d953a7dd70 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Tue, 15 Dec 2020 15:02:34 +0100
Subject: [PATCH] add phase_interpolation_min_vel setting

---
 Firmware/MotorControl/encoder.cpp | 3 +++
 Firmware/MotorControl/encoder.hpp | 1 +
 Firmware/odrive-interface.yaml    | 3 +++
 3 files changed, 7 insertions(+)

diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index 58bdba56..19c6fa09 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -792,6 +792,9 @@ bool Encoder::update() {
         vel_estimate_counts_ = 0.0f;  //align delta-sigma on zero to prevent jitter
         snap_to_zero_vel = true;
     }
+    if (std::abs(vel_estimate_counts_ / (float)config_.cpr) < config_.phase_interpolation_min_vel) {
+        snap_to_zero_vel = true;
+    }
 
     // Outputs from Encoder for Controller
     pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp
index d764582f..676c3418 100644
--- a/Firmware/MotorControl/encoder.hpp
+++ b/Firmware/MotorControl/encoder.hpp
@@ -35,6 +35,7 @@ public:
         int32_t direction = 0; // direction with respect to motor
         bool use_index_offset = true;
         bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
+        float phase_interpolation_min_vel = 0.0f;
         bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
         bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
         uint8_t hall_polarity = 0;
diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml
index cbae5039..c70c0a0f 100644
--- a/Firmware/odrive-interface.yaml
+++ b/Firmware/odrive-interface.yaml
@@ -981,6 +981,9 @@ interfaces:
           direction: int32
           pre_calibrated: {type: bool, c_setter: set_pre_calibrated}
           enable_phase_interpolation: bool
+          phase_interpolation_min_vel:
+            type: float32
+            unit: turns/s
           bandwidth: {type: float32, c_setter: set_bandwidth}
           calib_range: float32
           calib_scan_distance: float32