From f691536b3c2b90b7cedc8a852e5e715a05380d52 Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Wed, 2 Sep 2020 20:44:56 -0400
Subject: [PATCH] Fix DRV lock-up problem when using absolute encoders

---
 Firmware/MotorControl/axis.cpp    |  5 ++---
 Firmware/MotorControl/encoder.hpp |  3 ++-
 Firmware/MotorControl/main.cpp    | 19 ++++++++++++++++---
 3 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 18b2d96e..310a7a8a 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -85,10 +85,9 @@ static void step_cb_wrapper(void* ctx) {
 }
 
 
-// @brief Sets up all components of the axis,
-// such as gate driver and encoder hardware.
+// @brief Does Nothing
 void Axis::setup() {
-    motor_.setup();
+    // Does nothing - Motor and encoder setup called separately.
 }
 
 static void run_state_machine_loop_wrapper(void* ctx) {
diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp
index 15be6576..d100d168 100644
--- a/Firmware/MotorControl/encoder.hpp
+++ b/Firmware/MotorControl/encoder.hpp
@@ -5,9 +5,10 @@
 #error "This file should not be included directly. Include odrive_main.h instead."
 #endif
 
+
 class Encoder : public ODriveIntf::EncoderIntf {
 public:
-    const uint32_t MODE_FLAG_ABS = 0x100;
+    static constexpr uint32_t MODE_FLAG_ABS = 0x100;
 
     struct Config_t {
         Mode mode = MODE_INCREMENTAL;
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index 2809f5da..78bc9b37 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -262,15 +262,28 @@ int odrive_main(void) {
     // must happen after communication is initialized
     pwm_in_init();
 
-    // Setup hardware for all components
-    for (size_t i = 0; i < AXIS_COUNT; ++i) {
-        axes[i]->setup();
+    // Set up the CS pins for absolute encoders
+    for(auto& axis : axes){
+        if(axis->encoder_.config_.mode & Encoder::MODE_FLAG_ABS){
+            axis->encoder_.abs_spi_cs_pin_init();
+        }
     }
 
+    // Setup motors (DRV8301 SPI transactions here)
+    for(auto& axis : axes){
+        axis->motor_.setup();
+    }
+
+    // Setup encoders (Starts encoder SPI transactions)
     for(auto& axis : axes){
         axis->encoder_.setup();
     }
 
+    // Setup anything remaining in each axis
+    for(auto& axis : axes){
+        axis->setup();
+    }
+
     // Start PWM and enable adc interrupts/callbacks
     start_adc_pwm();