From 88b3ed42606f2d4eb909bec893c55e4a39472038 Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Sun, 26 Jul 2020 16:16:10 -0400
Subject: [PATCH] Optimize step/dir callback

---
 Firmware/.vscode/c_cpp_properties.json |  6 +++---
 Firmware/MotorControl/axis.cpp         | 10 ++++------
 Firmware/MotorControl/controller.cpp   |  3 ---
 Firmware/MotorControl/controller.hpp   |  5 ++++-
 4 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index 9c1aec97..1d5ab7cf 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -3,7 +3,7 @@
         {
             "name": "Win32",
             "includePath": [
-                "${workspaceRoot}/**"
+                "${workspaceFolder}/**"
             ],
             "defines": [
                 "STM32F405xx",
@@ -24,7 +24,7 @@
         {
             "name": "Linux",
             "includePath": [
-                "${workspaceRoot}/**"
+                "${workspaceFolder}/**"
             ],
             "defines": [
                 "STM32F405xx",
@@ -44,7 +44,7 @@
         {
             "name": "Mac",
             "includePath": [
-                "${workspaceRoot}/**"
+                "${workspaceFolder}/**"
             ],
             "defines": [
                 "STM32F405xx",
diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index f66f8f32..bc11d1e4 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -118,12 +118,10 @@ bool Axis::wait_for_current_meas() {
 
 // step/direction interface
 void Axis::step_cb() {
-    if (step_dir_active_) {
-        GPIO_PinState dir_pin = HAL_GPIO_ReadPin(dir_port_, dir_pin_);
-        float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
-        controller_.input_pos_ += dir * config_.counts_per_step;
-        controller_.input_pos_updated();
-    }
+    const GPIO_PinState dir_pin = (GPIO_PinState)(dir_port_->IDR & dir_pin_);
+    const int32_t dir = (1 - 2 * (int32_t)dir_pin) * step_dir_active_;
+    controller_.input_pos_ += dir * config_.counts_per_step;
+    controller_.input_pos_updated();
 };
 
 void Axis::load_default_step_dir_pin_config(
diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index c15b1cbe..7b3fe8ef 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -26,9 +26,6 @@ void Controller::set_error(Error error) {
 // Command Handling
 //--------------------------------
 
-void Controller::input_pos_updated() {
-    input_pos_updated_ = true;
-}
 
 bool Controller::select_encoder(size_t encoder_num) {
     if (encoder_num < AXIS_COUNT) {
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index 0b692450..b1f687dc 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -52,7 +52,10 @@ public:
     void reset();
     void set_error(Error error);
 
-    void input_pos_updated();
+    constexpr void input_pos_updated() {
+        input_pos_updated_ = true;
+    }
+
     bool select_encoder(size_t encoder_num);
 
     // Trajectory-Planned control