First WiThrottle prototype

DCC.cpp

@@ -62,6 +62,18 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) {
   DCCWaveform::mainTrack.schedulePacket(b, nB, 3);     // send packet 3 times
 }
 
+uint8_t DCC::getThrottleSpeed(int cab) {
+  int reg=lookupSpeedTable(cab);
+  if (reg<0) return -1;
+  return speedTable[reg].speedCode & 0x7F;
+}
+
+bool DCC::getThrottleDirection(int cab) {
+  int reg=lookupSpeedTable(cab);
+  if (reg<0) return false ;
+  return (speedTable[reg].speedCode & 0x80) !=0;
+}
+
 static void DCC::setFn( int cab, byte functionNumber, bool on) {
   if (cab<=0 || functionNumber<0 || functionNumber>28) return;
   int reg = lookupSpeedTable(cab);

DCC.h

@@ -36,6 +36,8 @@ class DCC {
 
   // Public DCC API functions
   static void setThrottle( uint16_t cab, uint8_t tSpeed, bool tDirection);
+  static uint8_t getThrottleSpeed(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);
   static void setFunction( int cab, byte fByte, byte eByte);

WiThrottle.cpp

@@ -0,0 +1,238 @@
+/*
+ * Truncated JMRI WiThrottle server implementation for DCC-EX command station
+ * Credit is due to  Valerie Valley RR https://sites.google.com/site/valerievalleyrr/
+ *  for showing how it could be done, but this code is very different to the original
+ *  implemenatatin as it is designed to run on the Arduino and not the ESP and is
+ *  also calling directly into the DCCEX Api rather than  simulating JMRI text commands.
+ * Refer JMRI WiFi Throttle Communications Protocol http://jmri.sourceforge.net/help/en/package/jmri/jmrit/withrottle/Protocol.shtml
+ * 
+ * 
+ * PROTOTYPE NOTES:
+ *  There will be one WiThrottle instance created for each WiThrottle client detected by the WifiInterface.
+ *  Some shortcuts have been taken and there are some things that are yet to be included:
+ *  e.g. Full response to adding a loco.
+ *  What to do about unknown turnouts.
+ *  Broadcasting to other WiThrottles when things change.  
+ *    -  Bear in mind that changes may have taken place due to  
+ *      other WiThrottles, OR JMRI commands received OR TPL automation. 
+ *    - I suggest that at the end of parse(), then anything that has changed and is of interest could  
+ *       be notified then.  (e.g loco speeds, directions or functions, turnout states.
+ *       
+ *  WiThrottle.h sets the max locos per client at 10, this is ok to increase but requires just an extra 3 bytes per loco per client.      
+*/
+#include <Arduino.h>
+#include "WiThrottle.h"
+#include "DCC.h"
+#include "DCCWaveform.h"
+#include "StringFormatter.h"
+#include "Turnouts.h"
+#include "DIAG.h"
+
+#define LOOPLOCOS(THROTTLECHAR, CAB)  for (int loco=0;loco<MAX_MY_LOCO;loco++) \
+      if (myLocos[loco].throttle==THROTTLECHAR && (CAB<0 || myLocos[loco].cab==CAB))
+
+WiThrottle * WiThrottle::firstThrottle=NULL;
+
+WiThrottle* WiThrottle::getThrottle(Print & stream, int wifiClient) {
+  for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle)  
+     if (wt->clientid==wifiClient) return wt;
+  return new WiThrottle(stream, wifiClient);
+}
+
+ // One instance of WiTHrottle per connected client, so we know what the locos are 
+ 
+WiThrottle::WiThrottle(Print & stream, int wificlientid) {
+   DIAG(F("\nCreating new WiThrottle for client %d\n"),wificlientid); 
+   nextThrottle=firstThrottle;
+   firstThrottle= this;
+   clientid=wificlientid;
+   for (int loco=0;loco<MAX_MY_LOCO; loco++) myLocos[loco].throttle='\0';
+  StringFormatter::send(stream,F("VN2.0\nRL0\nPPA%x\nPTT]\\[Turnouts}|{Turnout]\\[Closed}|{2]\\[Thrown}|{4\PTL"), DCCWaveform::mainTrack.getPowerMode()==POWERMODE::ON);
+
+   for(Turnout *tt=Turnout::firstTurnout;tt!=NULL;tt=tt->nextTurnout){
+        StringFormatter::send(stream,F("]\\[LT&d}|{%d}|{%d"), tt->data.id, tt->data.id, (bool)(tt->data.tStatus & STATUS_ACTIVE));
+      }
+  StringFormatter::send(stream,F("\n*10"));
+}
+
+WiThrottle::~WiThrottle() {
+  if (firstThrottle== this) {
+    firstThrottle=this->nextThrottle;
+    return;
+  }
+  for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) {
+    if (wt->nextThrottle==this) {
+      wt->nextThrottle=this->nextThrottle;
+      return;  
+     }
+  }
+}
+
+void WiThrottle::parse(Print & stream, char * cmd) {
+  heartBeat=millis();
+  DIAG(F("\nWiThrottle parse (%d) %s"),clientid, cmd);
+  
+   switch (cmd[0]) {
+       case '*':  // heartbeat control
+            if (cmd[1]=='+') heartBeatEnable=true;
+            else if (cmd[1]=='-') heartBeatEnable=false;
+            break;
+       case 'P':  
+            if (cmd[1]=='P' && cmd[2]=='A' )  {  //PPA power mode 
+              DCCWaveform::mainTrack.setPowerMode(cmd[3]=='1'?POWERMODE::ON:POWERMODE::OFF);
+              StringFormatter::send(stream, F("PPA%c"),cmd[3]);
+            }
+            else if (cmd[1]='T' && cmd[2]=='A') { // PTA accessory toggle 
+                // TODO... if we are given an address that is not a known Turnout...
+                // should we create one or just send the DCC message.                 
+                Turnout::activate(getInt(cmd+4),cmd[3]=='T');
+            }
+            break;
+       case 'N':  // Heartbeat (2)
+                StringFormatter::send(stream, F("*10")); // 10 second timeout  
+            break;
+       case 'M': // multithrottle
+            multithrottle(stream, cmd); 
+            break;
+   }           
+}
+int WiThrottle::getInt(char * cmd) {
+    int i=0;
+    while (cmd[0]>='0' && cmd[0]<='9') {
+      i=i*10 + (cmd[0]-'0');
+      cmd++;
+    }
+    return i;    
+}
+
+int WiThrottle::getLocoId(char * cmd) {
+    if (cmd[0]=='*') return -1;  // match all locos 
+    if (cmd[0]!='L' && cmd[0]!='S') return 0; // should not match any locos
+    return getInt(cmd+1); 
+}
+void WiThrottle::multithrottle(Print & stream, char* cmd){ 
+          char throttleChar=cmd[1];
+          int locoid=getLocoId(cmd+3); // -1 for *
+          char * aval=cmd;
+          while(*aval !=';' && *aval !='\0') aval++;
+          if (*aval) aval++;
+          
+       switch(cmd[2]) {
+          case '+':  // add loco
+               for (int loco=0;loco<MAX_MY_LOCO;loco++) {
+                  if (myLocos[loco].throttle=='\0') {
+                    myLocos[loco].throttle=throttleChar;
+                    myLocos[loco].cab=locoid;
+                    StringFormatter::send(stream, F("M%c+%c%d<;>\n"), throttleChar, cmd[3] ,locoid);
+                    // TODO... get known Fn states from DCC (need memoryStream improvements to handle data length)
+                    // for(fKey=0; fKey<29; fKey++)StringFormatter::send(stream,F("M%cA%c<;>F0&s\n"),throttleChar,cmd[3],fkey);
+                    StringFormatter::send(stream, F("M%c+%c%d<;>V0\n"), throttleChar, cmd[3], locoid);
+                    StringFormatter::send(stream, F("M%c+%c%d<;>R1\n"), throttleChar, cmd[3], locoid);
+                    StringFormatter::send(stream, F("M%c+%c%d<;>s1\n"), throttleChar, cmd[3], locoid);
+                    break;
+                  }
+               }
+               break;
+          case '-': // remove loco 
+                 LOOPLOCOS(throttleChar, locoid) {
+                     myLocos[loco].throttle='\0';
+                     DCC::setThrottle(myLocos[loco].cab,0,0);
+                     StringFormatter::send(stream, F("M%c-<;>\n"), throttleChar);
+                  }
+            
+            break;
+          case 'A':
+              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, char* aval, char throttleChar, int cab){
+    // Note cab=-1 for all cabs in the consist called throttleChar.  
+
+     switch (aval[0]) {
+           case 'V':  // Vspeed
+             { 
+              byte locospeed=getInt(aval+1);
+              LOOPLOCOS(throttleChar, cab) {
+                DCC::setThrottle(myLocos[loco].cab,locospeed, DCC::getThrottleDirection(myLocos[loco].cab));
+                }
+             } 
+            break;
+           case 'F': //F onOff function
+                {
+                  bool onOff=aval[1]=='1';
+                  int fKey = getInt(aval+2);
+                  LOOPLOCOS(throttleChar, cab) {
+                          DCC::setFn(myLocos[loco].cab, fKey,onOff);
+                   } 
+                }
+                break;  
+            case 'q':
+                if (aval[1]=='V') {   //qV
+                  LOOPLOCOS(throttleChar, cab) {              
+                    StringFormatter::send(stream,F("M%cAL%d<;>V%d"), throttleChar, myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab));
+                  }              
+                }
+                else if (aval[1]=='R') { // qR
+                  LOOPLOCOS(throttleChar, cab) {              
+                    StringFormatter::send(stream,F("M%cAL%d<;>R%d"), throttleChar, myLocos[loco].cab, DCC::getThrottleDirection(myLocos[loco].cab));
+                  }             
+                }     
+            break;    
+            case 'R':
+            { 
+              bool forward=aval[1]!='0';
+                 LOOPLOCOS(throttleChar, cab) {              
+                    DCC::setThrottle(myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab), forward);
+                  }
+            }        
+            break;      
+            case 'X':
+              //Emergency Stop  (TODO check we have the correct speed code here)  
+              LOOPLOCOS(throttleChar, cab) {
+                DCC::setThrottle(myLocos[loco].cab,1, DCC::getThrottleDirection(myLocos[loco].cab));
+                }
+               break;
+            case 'I': // Idle
+            case 'Q': // Quit
+              LOOPLOCOS(throttleChar, cab) {
+                DCC::setThrottle(myLocos[loco].cab,0, DCC::getThrottleDirection(myLocos[loco].cab));
+                }
+                break;
+            }               
+}
+
+void WiThrottle::loop() {
+  // for each WiThrottle, check the heartbeat
+  for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) 
+     wt->checkHeartbeat();
+}
+
+void WiThrottle::checkHeartbeat() {
+  if(millis()-heartBeat > HEARTBEAT_TIMEOUT*1000) {
+    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);
+        }
+    }
+  } 
+}

WiThrottle.h

@@ -0,0 +1,38 @@
+#ifndef WiThrottle_h
+#define WiTHrottle_h
+
+
+struct MYLOCO {
+    char throttle;
+    int cab;
+};
+
+class WiThrottle {
+  public:  
+    static void loop();
+    void parse(Print & stream, char * cmd);
+    static WiThrottle* getThrottle(Print & stream, int wifiClient); 
+
+  private: 
+    WiThrottle(Print & stream, int wifiClientId);
+    ~WiThrottle();
+   
+      static const int MAX_MY_LOCO=10;
+      static const int HEARTBEAT_TIMEOUT=10;
+      static WiThrottle* firstThrottle;
+      static int getInt(char * cmd);
+      static int getLocoId(char * cmd);
+      
+      WiThrottle* nextThrottle;
+      int clientid;
+       
+      MYLOCO myLocos[MAX_MY_LOCO];   
+      bool heartBeatEnable;
+      unsigned long heartBeat;
+      
+     void multithrottle(Print & stream, char* cmd);
+     void locoAction(Print & stream, char* aval, char throttleChar, int cab);
+     void accessory(Print & stream, char* cmd);
+     void checkHeartbeat();  
+};
+#endif

WifiInterface.cpp

@@ -2,7 +2,7 @@
 #include "Config.h"
 #include "DIAG.h"
 #include "StringFormatter.h"
-
+#include "WiThrottle.h"
  
 const char  PROGMEM READY_SEARCH[]  ="\r\nready\r\n";
 const char  PROGMEM OK_SEARCH[] ="\r\nOK\r\n";
@@ -78,6 +78,8 @@ bool WifiInterface::checkForOK( const int timeout, const char * waitfor, bool ec
 void WifiInterface::loop() {
     if (!connected) return; 
     
+    WiThrottle::loop();  // check heartbeats 
+    
     // read anything into a buffer, collecting info on the way  
     while (loopstate!=99 && Serial1.available()) { 
       int ch=Serial1.read();
@@ -116,7 +118,6 @@ void WifiInterface::loop() {
         } // switch 
     } // while
     if (loopstate!=99) return; 
-    // TODO remove > in data 
     streamer.write('\0');
 
     DIAG(F("\nWifiRead:%d:%s\n"),connectionId,buffer);
@@ -125,9 +126,15 @@ void WifiInterface::loop() {
     //  We know that parser will read the entire buffer before starting to write to it.
     //  Otherwise we would have to copy the buffer elsewhere and RAM is in short supply.
 
-    // TODO ... tell parser that callbacks are diallowed because we dont want to handle the async 
+    // TODO ... tell JMRI parser that callbacks are diallowed because we dont want to handle the async 
-    parser.parse(streamer,buffer);
+    
+    if (buffer[0]=='<')  parser.parse(streamer,buffer);
+    else WiThrottle::getThrottle(streamer, connectionId)->parse(streamer, buffer);
+
+       
     if (streamer.available()) { // there is a reply to send 
+        DIAG(F("WiFiInterface Responding (%d) %s\n"),connectionId,buffer);
+        
         StringFormatter::send(Serial1,F("AT+CIPSEND=%d,%d\r\n"),connectionId,streamer.available());
         streamer.write('\0');
         if (checkForOK(1000,PROMPT_SEARCH,true))  Serial1.print((char *) buffer);