From 24e5e648b8d369425135c39a1318dd61403fc430 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sun, 2 Oct 2022 13:40:46 +0200
Subject: [PATCH] improve current sense sampling on AVR

---
 DCCWaveform.cpp  |  5 +++--
 GITHUB_SHA.h     |  2 +-
 MotorDriver.cpp  |  2 +-
 TrackManager.cpp | 23 +++++++++++++++--------
 4 files changed, 20 insertions(+), 12 deletions(-)

diff --git a/DCCWaveform.cpp b/DCCWaveform.cpp
index 3010813..12ab87b 100644
--- a/DCCWaveform.cpp
+++ b/DCCWaveform.cpp
@@ -80,12 +80,13 @@ void DCCWaveform::interruptHandler() {
   // Set the signal state for both tracks
   TrackManager::setDCCSignal(sigMain);
   TrackManager::setPROGSignal(sigProg);
-  
+
+  TrackManager::sampleCurrent();
+
   // Move on in the state engine
   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/GITHUB_SHA.h b/GITHUB_SHA.h
index a08ac66..5696f3c 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "PORTX-HAL-202210020044"
+#define GITHUB_SHA "PORTX-HAL-202210021139Z"
diff --git a/MotorDriver.cpp b/MotorDriver.cpp
index dc70062..c99e63a 100644
--- a/MotorDriver.cpp
+++ b/MotorDriver.cpp
@@ -226,7 +226,7 @@ int MotorDriver::getCurrentRaw() {
   //  noInterrupts();
   //current = analogRead(currentPin)-senseOffset;
   current = sampleCurrent-senseOffset;
-  DIAG(F("%d %d"), current, sampleCurrentTimestamp);
+  //DIAG(F("%d %d"), current, sampleCurrentTimestamp);
   if ((millis() - sampleCurrentTimestamp) > 3)
     DIAG(F("Current sample old %d"), millis() - sampleCurrentTimestamp);
   //interrupts();
diff --git a/TrackManager.cpp b/TrackManager.cpp
index adce218..e596d8a 100644
--- a/TrackManager.cpp
+++ b/TrackManager.cpp
@@ -59,32 +59,39 @@ byte TrackManager::tempProgTrack=MAX_TRACKS+1;
  */
 void TrackManager::sampleCurrent() {
   static byte tr = 0;
+  byte trAtStart = tr;
   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
+    // for scope debug track[1]->setBrake(0);
     waiting = false;
     tr++;
-    if (tr >= MAX_TRACKS) tr = 0;
+    if (tr > lastTrack) tr = 0;
+    if (lastTrack < 2 || trackMode[tr] & TRACK_MODE_PROG) {
+      return; // We could continue but for prog track we
+              // rather do it in next interrupt beacuse
+              // that gives us well defined sampling point.
+              // For other tracks we care less unless we
+              // have only few (max 2) tracks.
+    }
   }
   if (!waiting) {
     // look for a valid track to sample or until we are around
-    while (count) {
+    while (true) {
       if (trackMode[tr] & ( TRACK_MODE_MAIN|TRACK_MODE_PROG|TRACK_MODE_DC|TRACK_MODE_DCX|TRACK_MODE_EXT )) {
 	track[tr]->startCurrentFromHW();
+	// for scope debug track[1]->setBrake(1);
 	waiting = true;
 	break;
       }
       tr++;
-      if (tr >= MAX_TRACKS) tr = 0;
-      count--;
-    }
-    if (count == 0) {
-      DIAG(F("WRONG"));
+      if (tr > lastTrack) tr = 0;
+      if (tr == trAtStart) // we are through and nothing found to do
+	return;
     }
   }
 }