From 744bfeb110d87b5caa534ac921ef9cad2654e772 Mon Sep 17 00:00:00 2001
From: PAJohnson <pajohns4@ncsu.edu>
Date: Fri, 26 Jun 2020 19:48:21 +0100
Subject: [PATCH] Un-removed custom setter for input_pos_ removed explicit
 casts where variables would be promoted anyway

---
 Firmware/MotorControl/controller.cpp | 8 ++++----
 Firmware/MotorControl/controller.hpp | 2 ++
 Firmware/MotorControl/encoder.cpp    | 8 ++++----
 Firmware/odrive-interface.yaml       | 1 +
 4 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index c8ea8f05..37b899f6 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -85,8 +85,8 @@ void Controller::start_anticogging_calibration() {
  */
 bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
     float pos_err = input_pos_ - pos_estimate;
-    if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold * (2.0f * M_PI) / (float)axis_->encoder_.config_.cpr &&
-        std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold * (2.0f * M_PI) / (float)axis_->encoder_.config_.cpr) {
+    if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold * (2.0f * M_PI) / axis_->encoder_.config_.cpr &&
+        std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold * (2.0f * M_PI) / axis_->encoder_.config_.cpr) {
         config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_torque_;
     }
     if (config_.anticogging.index < 3600) {
@@ -140,7 +140,7 @@ bool Controller::update(float* torque_setpoint_output) {
 
     // TODO also enable circular deltas for 2nd order filter, etc.
     if (pos_wrap_src_) {
-        float cpr = *pos_wrap_src_ * 2.0f * M_PI / ((float)axis_->encoder_.config_.cpr);
+        float cpr = *pos_wrap_src_ * 2.0f * M_PI / axis_->encoder_.config_.cpr;
         // Keep pos setpoint from drifting
         input_pos_ = fmodf_pos(input_pos_, cpr);
     }
@@ -235,7 +235,7 @@ bool Controller::update(float* torque_setpoint_output) {
         }
 
         if (pos_wrap_src_) {
-            float cpr = *pos_wrap_src_ * 2.0f * M_PI / ((float)axis_->encoder_.config_.cpr);
+            float cpr = *pos_wrap_src_ * 2.0f * M_PI / axis_->encoder_.config_.cpr;
             // Keep pos setpoint from drifting
             pos_setpoint_ = fmodf_pos(pos_setpoint_, cpr);
             // Circular delta
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index 743992c2..7bb8e270 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -95,6 +95,8 @@ public:
 
     bool anticogging_valid_ = false;
 
+    // custom setters
+    void set_input_pos(float value) { input_pos_ = value; input_pos_updated();}
 };
 
 #endif // __CONTROLLER_HPP
diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index ad9c02e5..300859e4 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -510,7 +510,7 @@ bool Encoder::update() {
     // discrete phase detector
     float delta_pos = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_));
     float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_));
-    delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
+    delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * config_.cpr);
     // pll feedback
     pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
     pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
@@ -523,9 +523,9 @@ bool Encoder::update() {
     }
 
     //expose pos/vel estimates in radians for Controller
-    pos_est_rad_ = pos_estimate_ * 2.0f * M_PI / (float)config_.cpr;
-    vel_est_rad_ = vel_estimate_ * 2.0f * M_PI / (float)config_.cpr;
-    pos_cpr_rad_ = pos_cpr_ * 2.0f * M_PI / (float)config_.cpr;
+    pos_est_rad_ = pos_estimate_ * 2.0f * M_PI / config_.cpr;
+    vel_est_rad_ = vel_estimate_ * 2.0f * M_PI / config_.cpr;
+    pos_cpr_rad_ = pos_cpr_ * 2.0f * M_PI / config_.cpr;
 
     //// run encoder count interpolation
     int32_t corrected_enc = count_in_cpr_ - config_.offset;
diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml
index d76a4ec8..1ab58698 100644
--- a/Firmware/odrive-interface.yaml
+++ b/Firmware/odrive-interface.yaml
@@ -596,6 +596,7 @@ interfaces:
       input_pos:
         type: float32
         unit: rad
+        c_setter: set_input_pos
       input_vel:
         type: float32
         unit: rad/s