From 016bc37b533c377756fb9af53483f12a3bf8b8ba Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sun, 5 Jun 2022 23:07:03 +0200
Subject: [PATCH] clean up getThrottleSpeed functions

---
 DCC.cpp          | 15 ++++++++++++++-
 DCC.h            |  3 ++-
 GITHUB_SHA.h     |  2 +-
 TrackManager.cpp | 15 ++++-----------
 WiThrottle.cpp   |  5 ++++-
 5 files changed, 25 insertions(+), 15 deletions(-)

diff --git a/DCC.cpp b/DCC.cpp
index 8e4114c..31ff0be 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -137,12 +137,25 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) {
   DCCWaveform::mainTrack.schedulePacket(b, nB, 0);
 }
 
-uint8_t DCC::getThrottleSpeed(int cab) {
+// returns speed steps 0 to 127 (1 == emergency stop)
+// or -1 on "loco not found"
+int8_t DCC::getThrottleSpeed(int cab) {
   int reg=lookupSpeedTable(cab);
   if (reg<0) return -1;
   return speedTable[reg].speedCode & 0x7F;
 }
 
+// returns speed code byte
+// or 128 (speed 0, dir forward) on "loco not found".
+uint8_t DCC::getThrottleSpeedByte(int cab) {
+  int reg=lookupSpeedTable(cab);
+  if (reg<0)
+    return 128;
+  return speedTable[reg].speedCode;
+}
+
+// returns direction on loco
+// or true/forward on "loco not found"
 bool DCC::getThrottleDirection(int cab) {
   int reg=lookupSpeedTable(cab);
   if (reg<0) return true;
diff --git a/DCC.h b/DCC.h
index 38ea980..b45c45e 100644
--- a/DCC.h
+++ b/DCC.h
@@ -56,7 +56,8 @@ public:
 
   // Public DCC API functions
   static void setThrottle(uint16_t cab, uint8_t tSpeed, bool tDirection);
-  static uint8_t getThrottleSpeed(int cab);
+  static int8_t getThrottleSpeed(int cab);
+  static uint8_t getThrottleSpeedByte(int cab);
   static bool getThrottleDirection(int cab);
   static void writeCVByteMain(int cab, int cv, byte bValue);
   static void writeCVBitMain(int cab, int cv, byte bNum, bool bValue);
diff --git a/GITHUB_SHA.h b/GITHUB_SHA.h
index 4e73c0f..bc11861 100644
--- a/GITHUB_SHA.h
+++ b/GITHUB_SHA.h
@@ -1 +1 @@
-#define GITHUB_SHA "TM-PORTX-20220525"
+#define GITHUB_SHA "TM-PORTX-20220605"
diff --git a/TrackManager.cpp b/TrackManager.cpp
index 472b263..87ea735 100644
--- a/TrackManager.cpp
+++ b/TrackManager.cpp
@@ -209,17 +209,10 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
 }
 
 void TrackManager::applyDCSpeed(byte t) {
-    
-        int16_t speed1=DCC::getThrottleSpeed(trackDCAddr[t]);
-        byte speedByte;
-        if (speed1<0) speedByte=0;
-        else {
-            speedByte=speed1;
-            bool direction=DCC::getThrottleDirection(trackDCAddr[t]);
-            if (trackMode[t]==TRACK_MODE_DCX) direction=!direction;  
-            if (direction) speedByte|=0x80;
-        }
-        track[t]->setDCSignal(speedByte);
+  uint8_t speedByte=DCC::getThrottleSpeedByte(trackDCAddr[t]);
+  if (trackMode[t]==TRACK_MODE_DCX)
+    speedByte = (speedByte & 0xF7) | ~(speedByte & 0x80); // Reverse highest bit
+  track[t]->setDCSignal(speedByte);
 }
 
 bool TrackManager::parseJ(Print *stream, int16_t params, int16_t p[])
diff --git a/WiThrottle.cpp b/WiThrottle.cpp
index a0b1fe3..5a835ce 100644
--- a/WiThrottle.cpp
+++ b/WiThrottle.cpp
@@ -421,7 +421,10 @@ void WiThrottle::locoAction(RingStream * stream, byte* aval, char throttleChar,
       bool forward=aval[1]!='0';
       LOOPLOCOS(throttleChar, cab) {
 	mostRecentCab=myLocos[loco].cab;
-	DCC::setThrottle(myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab), forward);
+	int8_t speed = DCC::getThrottleSpeed(myLocos[loco].cab);
+	if (speed < 0) //can not find any speed for this cab
+	  speed = 0;
+	DCC::setThrottle(myLocos[loco].cab, speed, forward);
 	// setThrottle will cause a broadcast so notification will be sent
       }
     }