Withrottle broadcast functions and speeds

CommandDistributor.cpp

@@ -87,8 +87,10 @@ void  CommandDistributor::broadcastTurnout(int16_t id, bool isClosed ) {
   // For DCC++ classic compatibility, state reported to JMRI is 1 for thrown and 0 for closed; 
   // The string below contains serial and Withrottle protocols which should
   // be safe for both types. 
-  StringFormatter::send(broadcastBufferWriter,
-  F("<H %d %d>\nPTA%c%d\n"),id, !isClosed, isClosed?'2':'4', id);
+  StringFormatter::send(broadcastBufferWriter,F("<H %d %d>\n"),id, !isClosed);
+#if defined(WIFI_ON) | defined(ETHERNET_ON)
+  StringFormatter::send(broadcastBufferWriter,F("PTA%c%d\n"), isClosed?'2':'4', id);
+#endif 
   broadcast();
   }  
  
@@ -97,6 +99,9 @@ void  CommandDistributor::broadcastTurnout(int16_t id, bool isClosed ) {
   StringFormatter::send(broadcastBufferWriter,F("<l %d %d %d %l>\n"),
      sp->loco,slot,sp->speedCode,sp->functions);    
   broadcast();
+#if defined(WIFI_ON) | defined(ETHERNET_ON)
+  WiThrottle::markForBroadcast(sp->loco);
+#endif 
 }
  
 void  CommandDistributor::broadcastPower() {

DCC.cpp

@@ -247,6 +247,12 @@ void DCC::updateGroupflags(byte & flags, int16_t functionNumber) {
   flags |= groupMask; 
 }
 
+uint16_t DCC::getFunctionMap(int cab) {
+  if (cab<=0) return 0;  // unknown pretend all functions off
+  int reg = lookupSpeedTable(cab);
+  return (reg<0)?0:speedTable[reg].functions;  
+}
+
 void DCC::setAccessory(int address, byte number, bool activate) {
   #ifdef DIAG_IO
   DIAG(F("DCC::setAccessory(%d,%d,%d)"), address, number, activate);

DCC.h

@@ -101,6 +101,7 @@ public:
   static void setFn(int cab, int16_t functionNumber, bool on);
   static int changeFn(int cab, int16_t functionNumber, bool pressed);
   static int  getFn(int cab, int16_t functionNumber);
+  static uint16_t getFunctionMap(int cab);
   static void updateGroupflags(byte &flags, int16_t functionNumber);
   static void setAccessory(int aAdd, byte aNum, bool activate);
   static bool writeTextPacket(byte *b, int nBytes);

DCCEXParser.cpp

@@ -298,10 +298,9 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
             break; // invalid direction code
 
         DCC::setThrottle(cab, tspeed, direction);
-        if (params == 4)
+        if (params == 4) // send obsolete format T response
             StringFormatter::send(stream, F("<T %d %d %d>\n"), p[0], p[2], p[3]);
-        else
-            StringFormatter::send(stream, F("<O>\n"));
+        // speed change will be broadcast anyway in new <l > format
         return;
     }
     case 'f': // FUNCTION <f CAB BYTE1 [BYTE2]>

WiThrottle.cpp

@@ -382,21 +382,14 @@ int WiThrottle::WiTToDCCSpeed(int WiTSpeed) {
 }
 
 void WiThrottle::loop(RingStream * stream) {
-  // for each WiThrottle, check the heartbeat
+  // for each WiThrottle, check the heartbeat and broadcast needed
   for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) 
-     wt->checkHeartbeat();
+     wt->checkHeartbeat(stream);
 
-   // TODO... any broadcasts to be done 
-   (void)stream; 
-   /* MUST follow this model in  this loop. 
-    *   stream->mark();
-    *   send 1 digit client id, and any data 
-    *   stream->commit() 
-     */
 
 }
 
-void WiThrottle::checkHeartbeat() {
+void WiThrottle::checkHeartbeat(RingStream * stream) {
   // if eStop time passed... eStop any locos still assigned to this client and then drop the connection
   if(heartBeatEnable && (millis()-heartBeat > ESTOP_SECONDS*1000)) {
   if (Diag::WITHROTTLE)  DIAG(F("%l WiThrottle(%d) eStop(%ds) timeout, drop connection"), millis(), clientid, ESTOP_SECONDS);
@@ -407,8 +400,53 @@ void WiThrottle::checkHeartbeat() {
       }
     }
     delete this;
+    return;
+   }
+   
+   // send any speed/direction/function changes for this loco 
+    LOOPLOCOS('*', -1) { 
+      if (myLocos[loco].throttle!='\0' && myLocos[loco].broadcastPending) {
+        stream->mark(clientid);
+        myLocos[loco].broadcastPending=false;
+        int cab=myLocos[loco].cab;
+        char lors=LorS(cab);
+        char throttle=myLocos[loco].throttle;
+        StringFormatter::send(stream,F("M%cA%c%d<;>V%d\n"),
+           throttle, lors , cab, DCCToWiTSpeed(DCC::getThrottleSpeed(cab)));
+        StringFormatter::send(stream,F("M%cA%c%d<;>R%d\n"), 
+          throttle, lors , cab, DCC::getThrottleDirection(cab));
+
+        // compare the DCC functionmap with the local copy and send changes  
+        uint16_t dccFunctionMap=DCC::getFunctionMap(cab);
+        uint16_t myFunctionMap=myLocos[loco].functionMap;
+        myLocos[loco].functionMap=dccFunctionMap;
+
+        // loop the maps sending any bit changed
+        // Loop is terminated as soon as no changes are left
+        for (byte fn=0;dccFunctionMap!=myFunctionMap;fn++) {
+             if ((dccFunctionMap&1) != (myFunctionMap&1)) {
+               StringFormatter::send(stream,F("M%cA%c%d<;>F%c%d\n"),
+                throttle, lors , cab, (dccFunctionMap&1)?'1':'0',fn);
+             } 
+             // shift just checked bit off end of both maps
+             dccFunctionMap>>=1;
+             myFunctionMap>>=1;
+          } 
+        stream->commit();     
       }
     }
+}
+
+void WiThrottle::markForBroadcast(int cab) {
+  for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) 
+      wt->markForBroadcast2(cab);
+}
+void WiThrottle::markForBroadcast2(int cab) {
+  LOOPLOCOS('*', cab) { 
+      myLocos[loco].broadcastPending=true;
+  }
+}
+
 
 char WiThrottle::LorS(int cab) {
     return (cab<=HIGHEST_SHORT_ADDR)?'S':'L';

WiThrottle.h

@@ -24,6 +24,8 @@
 struct MYLOCO {
     char throttle; //indicates which throttle letter on client, often '0','1' or '2'
     int cab; //address of this loco
+    bool broadcastPending;
+    uint16_t functionMap;
 };
 
 class WiThrottle {
@@ -31,6 +33,7 @@ class WiThrottle {
     static void loop(RingStream * stream);
     void parse(RingStream * stream, byte * cmd);
     static WiThrottle* getThrottle( int wifiClient); 
+    static void markForBroadcast(int cab);
       
   private: 
     WiThrottle( int wifiClientId);
@@ -62,8 +65,8 @@ class WiThrottle {
       void multithrottle(RingStream * stream, byte * cmd);
       void locoAction(RingStream * stream, byte* aval, char throttleChar, int cab);
       void accessory(RingStream *, byte* cmd);
-      void checkHeartbeat(); 
-
+      void checkHeartbeat(RingStream * stream); 
+      void markForBroadcast2(int cab);
        // callback stuff to support prog track acquire
        static RingStream * stashStream;
        static WiThrottle * stashInstance;