From c70ef3ffaad37de0287aa8f06106b60ab27444cb Mon Sep 17 00:00:00 2001
From: Mike S <CyberMerln@gmail.com>
Date: Tue, 30 Mar 2021 16:12:47 -0400
Subject: [PATCH] Final Fix for T4 interrupts

---
 DCCWaveform.cpp |  6 +++---
 MotorDriver.cpp | 31 +++++++++----------------------
 MotorDriver.h   | 13 ++++++++++++-
 3 files changed, 24 insertions(+), 26 deletions(-)

diff --git a/DCCWaveform.cpp b/DCCWaveform.cpp
index 68549c9..57f20a1 100644
--- a/DCCWaveform.cpp
+++ b/DCCWaveform.cpp
@@ -127,7 +127,7 @@ void DCCWaveform::checkPowerOverload(bool ackManagerActive) {
       break;
     case POWERMODE::ON:
       // Check current
-      lastCurrent=motorDriver->getCurrentRaw(isMainTrack);
+      lastCurrent=motorDriver->getCurrentRaw();
       if (lastCurrent < 0) {
 	  // We have a fault pin condition to take care of
 	  lastCurrent = -lastCurrent;
@@ -275,7 +275,7 @@ void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repea
 
 void DCCWaveform::setAckBaseline() {
       if (isMainTrack) return;
-      int baseline=motorDriver->getCurrentRaw(isMainTrack);
+      int baseline=motorDriver->getCurrentRaw();
       ackThreshold= baseline + motorDriver->mA2raw(ackLimitmA);
       if (Diag::ACK) DIAG(F("ACK baseline=%d/%dmA Threshold=%d/%dmA Duration between %dus and %dus"),
 			  baseline,motorDriver->raw2mA(baseline),
@@ -309,7 +309,7 @@ void DCCWaveform::checkAck() {
         return; 
     }
       
-    int current=motorDriver->getCurrentRaw(isMainTrack);
+    int current=motorDriver->getCurrentRaw();
     if (current > ackMaxCurrent) ackMaxCurrent=current;
     // An ACK is a pulse lasting between minAckPulseDuration and maxAckPulseDuration uSecs (refer @haba)
         
diff --git a/MotorDriver.cpp b/MotorDriver.cpp
index 1819ab7..d3f1f9c 100644
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -21,12 +21,6 @@
 #include "DCCTimer.h"
 #include "DIAG.h"
 
-#if defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41)
-#include <ADC.h>
-#include <ADC_util.h>
-ADC *adc = new ADC(); // adc object
-#endif
-
 #define setHIGH(fastpin)  *fastpin.inout |= fastpin.maskHIGH
 #define setLOW(fastpin)   *fastpin.inout &= fastpin.maskLOW
 #define isHIGH(fastpin)   (*fastpin.inout & fastpin.maskHIGH)
@@ -69,15 +63,6 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8
     senseOffset=analogRead(currentPin); // value of sensor at zero current
   }
 
-#if defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) 
-  if(currentPin != current_pin && currentPin!=UNUSED_PIN){
-    //adc->adc0->setReference(ADC_REFERENCE::REF_3V3);
-    adc->adc0->startContinuous(currentPin);
-  } else if(currentPin!=UNUSED_PIN){
-    //adc->adc1->setReference(ADC_REFERENCE::REF_3V3);
-    adc->adc1->startContinuous(currentPin);
-  }
-#endif
   faultPin=fault_pin;
   if (faultPin != UNUSED_PIN) {
     getFastPin(F("FAULT"),faultPin, 1 /*input*/, fastFaultPin);
@@ -154,16 +139,18 @@ bool MotorDriver::canMeasureCurrent() {
  * senseOffset handles the case where a shield returns values above or below 
  * a central value depending on direction.
  */
-int MotorDriver::getCurrentRaw(bool isMain) {
+int MotorDriver::getCurrentRaw() {
   if (currentPin==UNUSED_PIN) return 0; 
-  
   int current;
 #if defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41)
-  if(isMain) {
-    current = (uint16_t)adc->adc0->analogReadContinuous();
-  } else {
-    current = (uint16_t)adc->adc1->analogReadContinuous();
-  }
+  //if(isMain) {
+  //  current = (uint16_t)adc->adc0->analogReadContinuous();
+  //} else {
+  //  current = (uint16_t)adc->adc1->analogReadContinuous();
+  //}
+  bool irq = disableInterrupts();
+  current = analogRead(currentPin)-senseOffset;
+  enableInterrupts(irq);
 #elif defined(ARDUINO_TEENSY32) || defined(ARDUINO_TEENSY35)|| defined(ARDUINO_TEENSY36)
   unsigned char sreg_backup;
   sreg_backup = SREG;   /* save interrupt enable/disable state */
diff --git a/MotorDriver.h b/MotorDriver.h
index f7d8580..08db049 100644
--- a/MotorDriver.h
+++ b/MotorDriver.h
@@ -47,7 +47,7 @@ class MotorDriver {
     virtual void setPower( bool on);
     virtual void setSignal( bool high);
     virtual void setBrake( bool on);
-    virtual int  getCurrentRaw(bool isMain);
+    virtual int  getCurrentRaw();
     virtual unsigned int raw2mA( int raw);
     virtual int mA2raw( unsigned int mA);
     inline int getRawCurrentTripValue() {
@@ -73,5 +73,16 @@ class MotorDriver {
     int senseOffset;
     unsigned int tripMilliamps;
     int rawCurrentTripValue;
+#if defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41)
+    static bool disableInterrupts() {
+      uint32_t primask;
+      __asm__ volatile("mrs %0, primask\n" : "=r" (primask)::);
+      __disable_irq();
+      return (primask == 0) ? true : false;
+    }
+    static void enableInterrupts(bool doit) {
+      if (doit) __enable_irq();
+    }
+#endif
 };
 #endif