From c47e9b79ca50842427a1041e19475104e4921b01 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sun, 2 Oct 2022 00:43:46 +0200
Subject: [PATCH] do analogRead without need for noInterrupt - first test

---
 DCCTimerAVR.cpp  | 11 +++++++++--
 DCCWaveform.cpp  |  2 +-
 MotorDriver.cpp  | 49 ++++++++++++++++++++++++++++++++++++++++++++----
 MotorDriver.h    |  4 ++++
 TrackManager.cpp | 34 +++++++++++++++++++++++++++++++++
 TrackManager.h   |  7 ++++---
 6 files changed, 97 insertions(+), 10 deletions(-)

diff --git a/DCCTimerAVR.cpp b/DCCTimerAVR.cpp
index f5a3562..c82afe5 100644
--- a/DCCTimerAVR.cpp
+++ b/DCCTimerAVR.cpp
@@ -44,8 +44,15 @@ INTERRUPT_CALLBACK interruptHandler=0;
 
 void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
     interruptHandler=callback;
-    noInterrupts();          
-    ADCSRA = (ADCSRA & 0b11111000) | 0b00000100;   // speed up analogRead sample time 
+    noInterrupts();
+    // ADCSRA = (ADCSRA & 0b11111000) | 0b00000100;   // speed up analogRead sample time
+    // Set up ADC for free running mode
+    ADMUX=(1<<REFS0); //select AVCC as reference. We set MUX later
+    //ADCSRB = 0; //set free running mode
+    //ADCSRA = (1<<ADEN)|(1<< ADATE)|(1<<ADPS2)|(1<<ADPS0)|4;
+    ADCSRA = (1<<ADEN)|(1 << ADPS2);
+    //bitSet(ADCSRA, ADSC); //start the ADC
+
     TCCR1A = 0;
     ICR1 = CLOCK_CYCLES;
     TCNT1 = 0;   
diff --git a/DCCWaveform.cpp b/DCCWaveform.cpp
index d7dc26a..3010813 100644
--- a/DCCWaveform.cpp
+++ b/DCCWaveform.cpp
@@ -85,7 +85,7 @@ void DCCWaveform::interruptHandler() {
   mainTrack.state=stateTransform[mainTrack.state];    
   progTrack.state=stateTransform[progTrack.state];    
 
-
+  TrackManager::sampleCurrent();
   // WAVE_PENDING means we dont yet know what the next bit is
   if (mainTrack.state==WAVE_PENDING) mainTrack.interrupt2();  
   if (progTrack.state==WAVE_PENDING) progTrack.interrupt2();
diff --git a/MotorDriver.cpp b/MotorDriver.cpp
index 7088111..dc70062 100644
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -223,9 +223,13 @@ int MotorDriver::getCurrentRaw() {
 #ifdef ARDUINO_ARCH_ESP32
   current = local_adc1_get_raw(pinToADC1Channel(currentPin))-senseOffset;
 #else
-  noInterrupts();
-  current = analogRead(currentPin)-senseOffset;
-  interrupts();
+  //  noInterrupts();
+  //current = analogRead(currentPin)-senseOffset;
+  current = sampleCurrent-senseOffset;
+  DIAG(F("%d %d"), current, sampleCurrentTimestamp);
+  if ((millis() - sampleCurrentTimestamp) > 3)
+    DIAG(F("Current sample old %d"), millis() - sampleCurrentTimestamp);
+  //interrupts();
 #endif
   if (current<0) current=0-current;
   if ((faultPin != UNUSED_PIN)  && isLOW(fastFaultPin) && powerMode==POWERMODE::ON)
@@ -233,6 +237,42 @@ int MotorDriver::getCurrentRaw() {
   return current;
    
 }
+/*
+ * This should only be called in interrupt context
+ * Copies current value from HW to cached value in
+ * Motordriver.
+ */
+#pragma GCC push_options
+#pragma GCC optimize ("-O3")
+bool MotorDriver::sampleCurrentFromHW() {
+#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
+  const byte mask = 7;
+#else
+  const byte mask = 31;
+#endif
+  byte low, high;
+  //if (!bit_is_set(ADCSRA, ADIF))
+  if (bit_is_set(ADCSRA, ADSC))
+    return false;
+  //  if ((ADMUX & mask) != (currentPin - A0))
+  //    return false;
+  low = ADCL; //must read low before high
+  high = ADCH;
+  bitSet(ADCSRA, ADIF);
+  sampleCurrent = (high << 8) | low;
+  sampleCurrentTimestamp = millis();
+  return true;
+}
+void MotorDriver::startCurrentFromHW() {
+#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
+  const byte mask = 7;
+#else
+  const byte mask = 31;
+#endif
+  ADMUX=(1<<REFS0)|((currentPin-A0) & mask); //select AVCC as reference and set MUX
+  bitSet(ADCSRA,ADSC); // start conversion
+}
+#pragma GCC pop_options
 
 void MotorDriver::setDCSignal(byte speedcode) {
   if (brakePin == UNUSED_PIN)
@@ -296,7 +336,8 @@ int MotorDriver::getCurrentRawInInterrupt() {
 #ifdef ARDUINO_ARCH_ESP32 //On ESP we do all in loop() instead of in interrupt
   return getCurrentRaw();
 #else
-  return analogRead(currentPin)-senseOffset;
+  //return analogRead(currentPin)-senseOffset;
+  return getCurrentRaw();
 #endif
 }  
 
diff --git a/MotorDriver.h b/MotorDriver.h
index 5afc9e3..91106e9 100644
--- a/MotorDriver.h
+++ b/MotorDriver.h
@@ -183,6 +183,8 @@ class MotorDriver {
       isProgTrack = on;
     }
     void checkPowerOverload(bool useProgLimit, byte trackno);
+    bool sampleCurrentFromHW();
+    void startCurrentFromHW();
   private:
     bool isProgTrack = false; // tells us if this is a prog track
     void  getFastPin(const FSH* type,int pin, bool input, FASTPIN & result);
@@ -212,6 +214,8 @@ class MotorDriver {
     unsigned int sampleDelay;
     int progTripValue;
     int  lastCurrent;
+    unsigned long sampleCurrentTimestamp;
+    uint16_t sampleCurrent;
     int maxmA;
     int tripmA;
 
diff --git a/TrackManager.cpp b/TrackManager.cpp
index 9a3952c..adce218 100644
--- a/TrackManager.cpp
+++ b/TrackManager.cpp
@@ -54,6 +54,40 @@ int16_t TrackManager::joinRelay=UNUSED_PIN;
 byte TrackManager::tempProgTrack=MAX_TRACKS+1;
 #endif
 
+/*
+ * sampleCurrent() runs from Interrupt
+ */
+void TrackManager::sampleCurrent() {
+  static byte tr = 0;
+  static bool waiting = false;
+  byte count = MAX_TRACKS-1;
+
+  if (waiting) {
+    if (! track[tr]->sampleCurrentFromHW()) {
+      return; // no result, continue to wait
+    }
+    // found value, advance at least one track
+    waiting = false;
+    tr++;
+    if (tr >= MAX_TRACKS) tr = 0;
+  }
+  if (!waiting) {
+    // look for a valid track to sample or until we are around
+    while (count) {
+      if (trackMode[tr] & ( TRACK_MODE_MAIN|TRACK_MODE_PROG|TRACK_MODE_DC|TRACK_MODE_DCX|TRACK_MODE_EXT )) {
+	track[tr]->startCurrentFromHW();
+	waiting = true;
+	break;
+      }
+      tr++;
+      if (tr >= MAX_TRACKS) tr = 0;
+      count--;
+    }
+    if (count == 0) {
+      DIAG(F("WRONG"));
+    }
+  }
+}
 
 // The setup call is done this way so that the tracks can be in a list 
 // from the config... the tracks default to NULL in the declaration                 
diff --git a/TrackManager.h b/TrackManager.h
index 45325f9..90a9345 100644
--- a/TrackManager.h
+++ b/TrackManager.h
@@ -27,8 +27,9 @@
 #include "MotorDriver.h"
 // Virtualised Motor shield multi-track hardware Interface
 
-enum TRACK_MODE : byte {TRACK_MODE_OFF, TRACK_MODE_MAIN, TRACK_MODE_PROG,
-                        TRACK_MODE_DC, TRACK_MODE_DCX, TRACK_MODE_EXT};
+// use powers of two so we can do logical and/or on the track modes in if clauses.
+enum TRACK_MODE : byte {TRACK_MODE_OFF = 1, TRACK_MODE_MAIN = 2, TRACK_MODE_PROG = 4,
+                        TRACK_MODE_DC = 8, TRACK_MODE_DCX = 16, TRACK_MODE_EXT = 32};
 
 // These constants help EXRAIL macros say SET_TRACK(2,mode) OR SET_TRACK(C,mode) etc.
 const byte TRACK_NUMBER_0=0, TRACK_NUMBER_A=0;    
@@ -75,10 +76,10 @@ class TrackManager {
     static void setJoin(bool join);
     static bool isJoined() { return progTrackSyncMain;}
     static void setJoinRelayPin(byte joinRelayPin);
+    static void sampleCurrent();
     static int16_t joinRelay;
     static bool progTrackSyncMain;  // true when prog track is a siding switched to main
      static bool progTrackBoosted;   // true when prog track is not current limited
-   
     
   private:
     static void addTrack(byte t, MotorDriver* driver);