CV20 and 5ms queue

DCC.cpp

@@ -159,10 +159,10 @@ void DCC::setThrottle2( uint16_t cab, byte speedCode)  {
 
   }
   if ((speedCode & 0x7F) == 1) DCCQueue::scheduleEstopPacket(b, nB, 4, cab); // highest priority
-  else DCCQueue::scheduleDCCSpeedPacket( b, nB, 4, cab);
+  else DCCQueue::scheduleDCCSpeedPacket( b, nB, 0, cab);
 }
 
-void DCC::setFunctionInternal(int cab, byte byte1, byte byte2, byte count) {
+void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) {
   // DIAG(F("setFunctionInternal %d %x %x"),cab,byte1,byte2);
   byte b[4];
   byte nB = 0;
@@ -173,7 +173,7 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2, byte count) {
   if (byte1!=0) b[nB++] = byte1;
   b[nB++] = byte2;
 
-  DCCQueue::scheduleDCCPacket(b, nB, count);
+  DCCQueue::scheduleDCCPacket(b, nB, 0, cab);
 }
 
 // returns speed steps 0 to 127 (1 == emergency stop)
@@ -239,7 +239,7 @@ bool DCC::setFn( int cab, int16_t functionNumber, bool on) {
        b[nB++] = (functionNumber & 0x7F) | (on ? 0x80 : 0);  // low order bits and state flag
        b[nB++] = functionNumber >>7 ;  // high order bits
     }
-    DCCQueue::scheduleDCCPacket(b, nB, 4);
+    DCCQueue::scheduleDCCPacket(b, nB, 4,cab);
   }
   // We use the reminder table up to 28 for normal functions.
   // We use 29 to 31 for DC frequency as well so up to 28
@@ -419,7 +419,7 @@ void DCC::writeCVByteMain(int cab, int cv, byte bValue)  {
   b[nB++] = cv2(cv);
   b[nB++] = bValue;
 
-  DCCQueue::scheduleDCCPacket(b, nB, 4);
+  DCCQueue::scheduleDCCPacket(b, nB, 4,cab);
 }
 
 //
@@ -437,7 +437,7 @@ void DCC::readCVByteMain(int cab, int cv, ACK_CALLBACK callback)  {
   b[nB++] = cv2(cv);
   b[nB++] = 0;
 
-  DCCQueue::scheduleDCCPacket(b, nB, 4);
+  DCCQueue::scheduleDCCPacket(b, nB, 4,cab);
   Railcom::anticipate(cab,cv,callback);
 }
 
@@ -459,7 +459,7 @@ void DCC::writeCVBitMain(int cab, int cv, byte bNum, bool bValue)  {
   b[nB++] = cv2(cv);
   b[nB++] = WRITE_BIT | (bValue ? BIT_ON : BIT_OFF) | bNum;
 
-  DCCQueue::scheduleDCCPacket(b, nB, 4);
+  DCCQueue::scheduleDCCPacket(b, nB, 4,cab);
 }
 
 bool DCC::setTime(uint16_t minutes,uint8_t speed, bool suddenChange) {
@@ -619,6 +619,7 @@ const ackOp FLASH LOCO_ID_PROG[] = {
       V0, WACK, MERGE,
       V0, WACK, MERGE,
       VB, WACK, NAKSKIP, // bad read of cv20, assume its 0 
+      BAD20SKIP,     // detect invalid cv20 value and ignore 
       STASHLOCOID,   // keep cv 20 until we have cv19 as well.
       SETCV, (ackOp)19, 
       STARTMERGE,           // Setup to read cv 19
@@ -724,7 +725,9 @@ const ackOp FLASH CONSIST_ID_PROG[] = {
       BASELINE,
       SETCV,(ackOp)20,
       SETBYTEH,    // high byte to CV 20
-      WB,WACK,     // ignore dedcoder without cv20 support
+      WB,WACK,ITSKIP,
+      FAIL_IF_NONZERO_NAK, // fail if writing long address to decoder that cant support it
+      SKIPTARGET,
       SETCV,(ackOp)19,
       SETBYTEL,   // low byte of word
       WB,WACK,ITC1,   // If ACK, we are done - callback(1) means Ok
@@ -849,10 +852,12 @@ void DCC::loop()  {
   if (DCCWaveform::mainTrack.isReminderWindowOpen()) {
     // Now is a good time to choose a packet to be sent
     // Either highest priority from the queues or a reminder
-    if (!DCCQueue::scheduleNext()) {
+    if (!DCCQueue::scheduleNext(false)) {
+      // none pending, 
       issueReminders();
-      DCCQueue::scheduleNext(); // push through any just created reminder
+      DCCQueue::scheduleNext(true); // send any pending and force an idle if none 
     }
+
   }
 }
 
@@ -921,40 +926,23 @@ bool DCC::issueReminder(LOCO * slot) {
         break;
        case 1: // remind function group 1 (F0-F4)
           if (flags & FN_GROUP_1)
-#ifndef DISABLE_FUNCTION_REMINDERS
+      	    setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4)); // 100D DDDD
-	    setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4),0); // 100D DDDD
-#else
-	    setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4),2);
-          flags&= ~FN_GROUP_1;  // dont send them again
-#endif
           break;
        case 2: // remind function group 2 F5-F8
           if (flags & FN_GROUP_2)
-#ifndef DISABLE_FUNCTION_REMINDERS
+  	        setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F));                           // 1011 DDDD
-  	    setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F),0);                           // 1011 DDDD
-#else
-	    setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F),2);
-          flags&= ~FN_GROUP_2;  // dont send them again
-#endif
           break;
        case 3: // remind function group 3 F9-F12
           if (flags & FN_GROUP_3)
-#ifndef DISABLE_FUNCTION_REMINDERS
+	          setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F));                           // 1010 DDDD
-	    setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F),0);                           // 1010 DDDD
-#else
-	    setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F),2);
-          flags&= ~FN_GROUP_3;  // dont send them again
-#endif
           break;
        case 4: // remind function group 4 F13-F20
           if (flags & FN_GROUP_4)
-	    setFunctionInternal(loco,222, ((functions>>13)& 0xFF),2);
+	          setFunctionInternal(loco,222, ((functions>>13)& 0xFF));
-          flags&= ~FN_GROUP_4;  // dont send them again
           break;
        case 5: // remind function group 5 F21-F28
           if (flags & FN_GROUP_5)
-	    setFunctionInternal(loco,223, ((functions>>21)& 0xFF),2);
+	          setFunctionInternal(loco,223, ((functions>>21)& 0xFF));
-          flags&= ~FN_GROUP_5;  // dont send them again
           break;
       }
       loopStatus++;

DCC.h

@@ -130,7 +130,7 @@ private:
   static byte defaultMomentumA;  // Accelerating
   static byte defaultMomentumD;  // Accelerating
   static void setThrottle2(uint16_t cab, uint8_t speedCode);
-  static void setFunctionInternal(int cab, byte fByte, byte eByte, byte count);
+  static void setFunctionInternal(int cab, byte fByte, byte eByte);
   static bool issueReminder(LOCO * slot);
   static LOCO* nextLocoReminder;
   static FSH *shieldName;

DCCACK.cpp

@@ -347,6 +347,20 @@ void DCCACK::loop() {
             opcode=GETFLASH(ackManagerProg);
           }
           break;
+     case BAD20SKIP:
+          if (ackManagerByte > 120) {
+            // skip to SKIPTARGET if cv20 is >120 (some decoders respond with 255)
+            if (Diag::ACK) DIAG(F("XX cv20=%d "),ackManagerByte);
+            while (opcode!=SKIPTARGET) {
+              ackManagerProg++;
+              opcode=GETFLASH(ackManagerProg);
+            }
+          }
+          break; 
+     case FAIL_IF_NONZERO_NAK: // fail if writing long address to decoder that cant support it
+          if (ackManagerByte==0) break;
+          callback(-4);  
+          return;           
      case SKIPTARGET:
           break;
      default:

DCCACK.h

@@ -58,6 +58,8 @@ enum ackOp : byte
   ITSKIP,           // skip to SKIPTARGET if ack true
   NAKSKIP,          // skip to SKIPTARGET if ack false
   COMBINE1920,      // combine cvs 19 and 20 and callback
+  BAD20SKIP,        // skip to SKIPTARGET if cv20 is >120 (some decoders respond with 255) 
+  FAIL_IF_NONZERO_NAK, // fail if writing long address to decoder that cant support it
   SKIPTARGET = 0xFF // jump to target
 };
 

DCCEXParser.cpp

@@ -169,8 +169,10 @@ int16_t DCCEXParser::splitValues(int16_t result[MAX_COMMAND_PARAMS], byte *cmd,
                 break;
             if (hot == '\0')
 	      return -1;
-	    if (hot == '>')
+	    if (hot == '>') {
+	      *remainingCmd = '\0';  // terminate the cmd string with 0 instead of '>'
 	      return parameterCount;
+	    }
             state = 2;
             continue;
 
@@ -272,18 +274,23 @@ void DCCEXParser::parse(const FSH * cmd) {
 // See documentation on DCC class for info on this section
 
 void DCCEXParser::parse(Print *stream,  byte *com,  RingStream *ringStream) {
-  // This function can get stings of the form "<C OMM AND>" or "C OMM AND"
+  // This function can get stings of the form "<C OMM AND>" or "C OMM AND>"
-  // found is true first after the leading "<" has been passed
+  // found is true first after the leading "<" has been passed which results
+  // in parseOne() getting c="C OMM AND>"
+  byte *cForLater = NULL;
   bool found = (com[0] != '<');
   for (byte *c=com; c[0] != '\0'; c++) {
     if (found) {
-      parseOne(stream, c,  ringStream);
+      cForLater = c;
       found=false;
     }
-    if (c[0] == '<')
+    if (c[0] == '<') {
+      if (cForLater) parseOne(stream, cForLater, ringStream);
       found = true;
     }
   }
+  if (cForLater) parseOne(stream, cForLater, ringStream);
+}
 
 void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
 {
@@ -1517,6 +1524,7 @@ void DCCEXParser::callback_Wloco(int16_t result)
 
 void DCCEXParser::callback_Wconsist(int16_t result)
 {
+    if (result==-4) DIAG(F("Long Consist %d not supported by decoder"),stashP[1]);
     if (result==1) result=stashP[1]; // pick up original requested id from command
     StringFormatter::send(getAsyncReplyStream(), F("<w CONSIST %d%S>\n"),
      result, stashP[2]=="REVERSE"_hk ? F(" REVERSE") : F(""));

DCCQueue.cpp

@@ -18,7 +18,13 @@
  *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-
+/* What does this queue manager do:
+  1. It provides a high priority queue and a low priority queue.
+  2. It manages situations where multiple loco speed commands are in the queue.
+  3. It allows an ESTOP to jump the queue and eliminate any outstanding speed commands that would later undo the stop.
+  4. It allows for coil on/off accessory commands to be synchronized to a given time delay.
+  5. It prevents transmission of sequential packets to the same loco id 
+  */
 #include "Arduino.h"
 #include "defines.h"
 #include "DCCQueue.h"
@@ -29,6 +35,8 @@
 DCCQueue* DCCQueue::lowPriorityQueue=new DCCQueue();
 DCCQueue* DCCQueue::highPriorityQueue=new DCCQueue();
 PendingSlot* DCCQueue::recycleList=nullptr;
+uint16_t DCCQueue::lastSentPacketLocoId=0; // used to prevent two packets to the same loco in a row
+
 
     DCCQueue::DCCQueue() {
         head=nullptr;
@@ -55,8 +63,8 @@ PendingSlot* DCCQueue::recycleList=nullptr;
     }
     
     // Packet joins end of low priority queue.
-    void DCCQueue::scheduleDCCPacket(byte* packet, byte length, byte repeats) {
+    void DCCQueue::scheduleDCCPacket(byte* packet, byte length, byte repeats, uint16_t loco) {
-        lowPriorityQueue->addQueue(getSlot(NORMAL_PACKET,packet,length,repeats,0));
+        lowPriorityQueue->addQueue(getSlot(NORMAL_PACKET,packet,length,repeats,loco));
     }
     
     // Packet replaces existing loco speed packet or joins end of high priority queue. 
@@ -65,6 +73,10 @@ PendingSlot* DCCQueue::recycleList=nullptr;
         for (auto p=highPriorityQueue->head;p;p=p->next) {
             if (p->locoId==loco) {
                 // replace existing packet
+                if (length>sizeof(p->packet)) {
+                    DIAG(F("DCC bad packet length=%d"),length);
+                    length=sizeof(p->packet); // limit to size of packet
+                }
                 memcpy(p->packet,packet,length);
                 p->packetLength=length;
                 p->packetRepeat=repeats;
@@ -84,11 +96,12 @@ PendingSlot* DCCQueue::recycleList=nullptr;
         
         // DIAG(F("DCC ESTOP loco=%d"),loco);
        
-        // kill any existing throttle packets for this loco
+        // kill any existing throttle packets for this loco (or all locos if broadcast)
+        // this will also remove any estop packets for this loco (or all locos if broadcast) but they will be replaced
         PendingSlot * previous=nullptr;
         auto p=highPriorityQueue->head;
         while(p) {
-            if (loco==0 || p->locoId==loco) {
+            if (p->type!=ACC_OFF_PACKET && (loco==0 || p->locoId==loco)) {
                 // drop this packet from the highPriority   queue 
                 if (previous) previous->next=p->next;
                 else highPriorityQueue->head=p->next;
@@ -107,9 +120,9 @@ PendingSlot* DCCQueue::recycleList=nullptr;
         highPriorityQueue->jumpQueue(getSlot(NORMAL_PACKET,packet,length,repeats,0));
     }
 
-    // Accessory gate-On Packet joins end of queue as normal.
+    // Accessory coil-On Packet joins end of queue as normal.
     // When dequeued, packet is retained at start of queue 
-    // but modified to gate-off and given the delayed start.
+    // but modified to coil-off and given the delayed start.
     // getNext will ignore this packet until the requested start time. 
     void DCCQueue::scheduleAccOnOffPacket(byte* packet, byte length, byte repeats,int16_t delayms) {
         auto p=getSlot(ACC_ON_PACKET,packet,length,repeats,0);
@@ -118,50 +131,69 @@ PendingSlot* DCCQueue::recycleList=nullptr;
     };
 
     
-    // Obtain packet (fills packet, length and repeats) 
+    // Schedule the next dcc packet from the queues or an idle packet if none pending.
-    // returns 0 length if nothing in queue.
+    const byte idlePacket[] = {0xFF, 0x00};
 
-    bool DCCQueue::scheduleNext() {
+    bool DCCQueue::scheduleNext(bool force) {
-        // check high priority queue first
+        if (highPriorityQueue->scheduleNextInternal()) return true;
-        if (!DCCWaveform::mainTrack.isReminderWindowOpen()) return false;
+        if (lowPriorityQueue->scheduleNextInternal()) return true;
-        PendingSlot* previous=nullptr;
+        if (force) {
-        for (auto p=highPriorityQueue->head;p;p=p->next) {
+            // This will arise when there is nothing available to be sent that will not compromise the rules
-            // skip over pending ACC_OFF packets which are still delayed
+            // typically this will only happen when there is only one loco in the reminders as the closely queued 
-            if (p->type == ACC_OFF_PACKET && millis()<p->startTime) continue;
+            // speed and function reminders must be separated by at least one packet not sent to that loco.
-            // use this slot
+            DCCWaveform::mainTrack.schedulePacket(idlePacket,sizeof(idlePacket),0);
-            DCCWaveform::mainTrack.schedulePacket(p->packet,p->packetLength,p->packetRepeat);
+            lastSentPacketLocoId=0;
-            // remove this slot from the queue 
-            if (previous) previous->next=p->next;
-            else highPriorityQueue->head=p->next;
-            if (!highPriorityQueue->head) highPriorityQueue->tail=nullptr;
-            
-            // and recycle it.
-            recycle(p);     
             return true;
         }
+        return false;
+    }
 
-        // No high priopity packets found, check low priority queue
+    bool DCCQueue::scheduleNextInternal() {
-        auto p=lowPriorityQueue->head;
+        PendingSlot* previous=nullptr;
-        if (!p) return false;  // nothing in queues
+        for (auto p=head;p;previous=p,p=p->next) {
-        
+            // skip over pending ACC_OFF packets which are still delayed
-        // schedule first packet in queue 
+            if (p->type == ACC_OFF_PACKET && millis()<p->startTime) continue;
+            if (p->locoId) {
+                // Prevent two consecutive packets to the same loco. 
+                // this also means repeats cant be done by waveform 
+                if (p->locoId==lastSentPacketLocoId) continue;  // try again later 
+                DCCWaveform::mainTrack.schedulePacket(p->packet,p->packetLength,0);
+                lastSentPacketLocoId=p->locoId;
+                if (p->packetRepeat) {
+                    p->packetRepeat--;
+                    return true; // leave this packet in the queue
+                }
+            }
+            else {
+                // Non loco packets can repeat automatically
                 DCCWaveform::mainTrack.schedulePacket(p->packet,p->packetLength,p->packetRepeat);
+                lastSentPacketLocoId=0;
+            }
 
-        // remove from queue
+            // remove this slot from the queue 
-        lowPriorityQueue->head=p->next;
+            if (previous) previous->next=p->next;
-        if (!lowPriorityQueue->head) lowPriorityQueue->tail=nullptr;
+            else head=p->next;
+            if (!head) tail=nullptr;
             
+            // special cases handling 
             if (p->type == ACC_ON_PACKET) {
                 // convert to a delayed off packet and jump the high priority queue
                 p->type= ACC_OFF_PACKET;
                 p->packet[1]  &= ~0x08; // set C to 0 (gate off) 
                 p->startTime=millis()+p->delayOff;
                 highPriorityQueue->jumpQueue(p);
-                }
-        else recycle(p);  // recycle this slot
                 return true;
             }
              
+            // Recycle packet just consumed
+            recycle(p);     
+            return true;
+        }
+        
+        // No packets found
+        return false;
+    }
+    
     // obtain and initialise slot for a PendingSlot.
     PendingSlot*  DCCQueue::getSlot(PendingType type, byte* packet, byte length, byte repeats,uint16_t loco) {
         PendingSlot * p; 
@@ -170,13 +202,30 @@ PendingSlot* DCCQueue::recycleList=nullptr;
             recycleList=p->next;
         }
         else { 
-            DIAG(F("New DCC queue slot"));
+            static int16_t created=0;
+            int16_t q1=0;
+            int16_t q2=0;
+            for (auto p=highPriorityQueue->head;p;p=p->next) q1++;
+            for (auto p=lowPriorityQueue->head;p;p=p->next) q2++;
+            bool leak=(q1+q2)!=created;
+            DIAG(F("New DCC queue slot type=%d length=%d loco=%d q1=%d q2=%d created=%d"),
+                   type,length,loco,q1,q2, created);
+            if (leak) {
+                for (auto p=highPriorityQueue->head;p;p=p->next) DIAG(F("q1 %d %d"),p->type,p->locoId);
+                for (auto p=lowPriorityQueue->head;p;p=p->next) DIAG(F("q2 %d %d"),p->type,p->locoId);
+            }       
             p=new PendingSlot; // need a queue entry
+            created++; 
         }
         p->next=nullptr;
         p->type=type;
         p->packetLength=length;
         p->packetRepeat=repeats; 
+        if (length>sizeof(p->packet)) {
+            DIAG(F("DCC bad packet length=%d"),length);
+            length=sizeof(p->packet); // limit to size of packet
+        }
+        p->startTime=0; // not used for loco packets
         memcpy((void*)p->packet,packet,length);
         p->locoId=loco;
         return p; 

DCCQueue.h

@@ -23,7 +23,7 @@
 #include "Arduino.h"
 #include "DCCWaveform.h"
 
-enum PendingType:byte {NORMAL_PACKET,ACC_ON_PACKET,ACC_OFF_PACKET,DEAD_PACKET};
+enum PendingType:byte {NORMAL_PACKET,SPEED_PACKET,FUNCTION_PACKET,ACC_ON_PACKET,ACC_OFF_PACKET,DEAD_PACKET};
   struct PendingSlot {
       PendingSlot* next; 
       PendingType type;
@@ -32,8 +32,7 @@ enum PendingType:byte {NORMAL_PACKET,ACC_ON_PACKET,ACC_OFF_PACKET,DEAD_PACKET};
       byte packet[MAX_PACKET_SIZE];
       
       union { // use depends on packet type
-        uint16_t locoId;    // NORMAL_PACKET .. only set >0 for speed change packets 
+        uint16_t locoId;    // SPEED & FUNCTION packets  
-                            // so they can be easily discarded if an estop jumps the queue.
         uint16_t delayOff;  // ACC_ON_PACKET delay to apply between on/off
         uint32_t startTime; // ACC_OFF_PACKET time (mS) to transmit 
       };
@@ -44,7 +43,7 @@ class DCCQueue {
     
     
     // Non-speed packets are queued in the main queue
-    static void scheduleDCCPacket(byte* packet, byte length, byte repeats);
+    static void scheduleDCCPacket(byte* packet, byte length, byte repeats, uint16_t loco=0);
 
     // Speed packets are queued in the high priority queue
     static void scheduleDCCSpeedPacket(byte* packet, byte length, byte repeats, uint16_t loco);
@@ -58,16 +57,16 @@ class DCCQueue {
     static void scheduleAccOnOffPacket(byte* packet, byte length, byte repeats,int16_t delayms);
 
   
-    // Schedules a main track packet from the queues if none pending.  
+    // Schedules a main track packet from the queues.
-    // returns true if a packet was scheduled.
+    static bool scheduleNext(bool force); 
-    static bool scheduleNext(); 
 
   private:
-  
+    bool scheduleNextInternal(); 
   // statics to manage high and low priority queues and recycleing of PENDINGs
     static PendingSlot* recycleList;
     static DCCQueue* highPriorityQueue;
     static DCCQueue* lowPriorityQueue;
+    static uint16_t lastSentPacketLocoId; // used to prevent two packets to the same loco in a row
 
     DCCQueue();