From 2a22602e821abc6d8189359059494e011828e933 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sat, 2 Nov 2019 14:05:22 -0700
Subject: [PATCH] add dirty fix for setpoint in cpr

---
 Firmware/MotorControl/controller.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index f1687494..107fa5ed 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -136,6 +136,13 @@ bool Controller::update(float* current_setpoint_output) {
         anticogging_calibration(axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_);
     }
 
+    // TODO also enable circular deltas for 2nd order filter, etc.
+    if (pos_wrap_src_) {
+        float cpr = *pos_wrap_src_;
+        // Keep pos setpoint from drifting
+        input_pos_ = fmodf_pos(input_pos_, cpr);
+    }
+
     // Update inputs
     switch (config_.input_mode) {
         case INPUT_MODE_INACTIVE: {