From 20e8715fc6e6ec6b9208e57e0da0d8b40d9ee4ad Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Mon, 29 Jun 2020 13:03:08 +0100
Subject: [PATCH] WiThrottle ACTUALLY WORKS

---
 DCC.cpp        | 13 +++++++------
 WiThrottle.cpp | 34 ++++++++++------------------------
 2 files changed, 17 insertions(+), 30 deletions(-)

diff --git a/DCC.cpp b/DCC.cpp
index e1c6d39..c0c01dc 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -38,7 +38,8 @@ void DCC::setThrottle2( uint16_t cab, byte speedCode)  {
 
   uint8_t b[4];
   uint8_t nB = 0;
-
+  // DIAG(F("\nsetSpeedInternal %d %x"),cab,speedCode);
+  
   if (cab > 127)
     b[nB++] = highByte(cab) | 0xC0;    // convert train number into a two-byte address
   b[nB++] = lowByte(cab);
@@ -74,8 +75,8 @@ bool DCC::getThrottleDirection(int cab) {
   return (speedTable[reg].speedCode & 0x80) !=0;
 }
 
-static void DCC::setFn( int cab, byte functionNumber, bool on) {
-  if (cab<=0 || functionNumber<0 || functionNumber>28) return;
+ void DCC::setFn( int cab, byte functionNumber, bool on) {
+  if (cab<=0 || functionNumber>28) return;
   int reg = lookupSpeedTable(cab);
   if (reg<0) return;  
     // set the function on/off in the functions and set the group flag to
@@ -94,9 +95,9 @@ static void DCC::setFn( int cab, byte functionNumber, bool on) {
 
 void DCC::setAccessory(int address, byte number, bool activate) {
   // use masks to detect wrong values and do nothing
-  if(address != address & 511)
+  if(address != (address & 511))
     return;
-  if(number != number & 3)
+  if(number != (number & 3))
     return;
   byte b[2];
 
@@ -310,7 +311,7 @@ bool DCC::issueReminder(int reg) {
          break;
        case 1: // remind function group 1 (F0-F4)
           if (flags & FN_GROUP_1) 
-              setFunctionInternal(loco,0, 128 + ((functions>>1)& 0x0F) | (functions & 0x01)<<4);   
+              setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4));   
           break;     
        case 2: // remind function group 2 F5-F8
           if (flags & FN_GROUP_2) 
diff --git a/WiThrottle.cpp b/WiThrottle.cpp
index 1ca9d72..b7edfac 100644
--- a/WiThrottle.cpp
+++ b/WiThrottle.cpp
@@ -115,8 +115,9 @@ void WiThrottle::multithrottle(Print & stream, byte * cmd){
           int locoid=getLocoId(cmd+3); // -1 for *
           byte * aval=cmd;
           while(*aval !=';' && *aval !='\0') aval++;
-          if (*aval) aval++;
-          
+          if (*aval) aval+=2;  // skip ;>
+
+       DIAG(F("\nMultithrottle aval=%c cab=%d"), aval[0],locoid);    
        switch(cmd[2]) {
           case '+':  // add loco
                for (int loco=0;loco<MAX_MY_LOCO;loco++) {
@@ -145,28 +146,10 @@ void WiThrottle::multithrottle(Print & stream, byte * cmd){
               locoAction(stream,aval, throttleChar, locoid);
             }
 }
-      
-
-
-
-/*** TODO provide add feedback ***
-
-void locoAdd(String th, String actionKey, int i) {
-  LocoThrottle[Throttle] = actionKey;
-  client[i].println("M"+th+"+"+actionKey+"<;>");
-  for(fKey=0; fKey<29; fKey++){
-    LocoState[Throttle][fKey] =0;
-    client[i].println("M"+th+"A"+actionKey+"<;>F0"+String(fKey));
-  }
-  client[i].println("M"+th+"+"+actionKey+"<;>V0");
-  client[i].println("M"+th+"+"+actionKey+"<;>R1");
-  client[i].println("M"+th+"+"+actionKey+"<;>s1");
-}
-*********/
 
 void WiThrottle::locoAction(Print & stream, byte* aval, char throttleChar, int cab){
     // Note cab=-1 for all cabs in the consist called throttleChar.  
-
+    DIAG(F("\nLoco Action aval=%c throttleChar=%c, cab=%d"), aval[0],throttleChar, cab);
      switch (aval[0]) {
            case 'V':  // Vspeed
              { 
@@ -228,11 +211,14 @@ void WiThrottle::loop() {
 
 void WiThrottle::checkHeartbeat() {
   if(millis()-heartBeat > HEARTBEAT_TIMEOUT*1000) {
+    // Haertbeat missed... STOP all locos for this client
     for (int loco=0;loco<MAX_MY_LOCO;loco++) {
         if (myLocos[loco].throttle!='\0') {
           DCC::setThrottle(myLocos[loco].cab, 1, DCC::getThrottleDirection(myLocos[loco].cab));
-   // TODO  Which stream??? Multiple clients ??          StringFormatter::send(stream,F("M%cAL%d<;>V0"),myLocos[loco].throttle,myLocos[loco].cab);
-        }
+         }
+    }
+  }
+  else {
+      // TODO  Check if anything has changed on my locos since last notified! 
     }
-  } 
 }