From 2e51dc73fd824477510c42b8d40cb3777c8c87a3 Mon Sep 17 00:00:00 2001 From: Asbelos Date: Wed, 23 Apr 2025 09:12:57 +0100 Subject: [PATCH] CV20 and 5ms queue --- DCC.cpp | 52 ++++++++----------- DCC.h | 2 +- DCCACK.cpp | 14 +++++ DCCACK.h | 2 + DCCEXParser.cpp | 18 +++++-- DCCQueue.cpp | 133 +++++++++++++++++++++++++++++++++--------------- DCCQueue.h | 15 +++--- 7 files changed, 148 insertions(+), 88 deletions(-) diff --git a/DCC.cpp b/DCC.cpp index b4b6fc1..9cbc157 100644 --- a/DCC.cpp +++ b/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),0); // 100D DDDD -#else - setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4),2); - flags&= ~FN_GROUP_1; // dont send them again -#endif + setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4)); // 100D DDDD 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),0); // 1011 DDDD -#else - setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F),2); - flags&= ~FN_GROUP_2; // dont send them again -#endif + setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F)); // 1011 DDDD 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),0); // 1010 DDDD -#else - setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F),2); - flags&= ~FN_GROUP_3; // dont send them again -#endif + setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F)); // 1010 DDDD break; case 4: // remind function group 4 F13-F20 if (flags & FN_GROUP_4) - setFunctionInternal(loco,222, ((functions>>13)& 0xFF),2); - flags&= ~FN_GROUP_4; // dont send them again + setFunctionInternal(loco,222, ((functions>>13)& 0xFF)); break; case 5: // remind function group 5 F21-F28 if (flags & FN_GROUP_5) - setFunctionInternal(loco,223, ((functions>>21)& 0xFF),2); - flags&= ~FN_GROUP_5; // dont send them again + setFunctionInternal(loco,223, ((functions>>21)& 0xFF)); break; } loopStatus++; diff --git a/DCC.h b/DCC.h index ac9ab3e..af8c21b 100644 --- a/DCC.h +++ b/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; diff --git a/DCCACK.cpp b/DCCACK.cpp index 7a43cd1..0ae5224 100644 --- a/DCCACK.cpp +++ b/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: diff --git a/DCCACK.h b/DCCACK.h index c50dbbd..3a6148d 100644 --- a/DCCACK.h +++ b/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 }; diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp index 20181d5..3879640 100644 --- a/DCCEXParser.cpp +++ b/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,17 +274,22 @@ 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 "" or "C OMM AND" - // found is true first after the leading "<" has been passed + // This function can get stings of the form "" or "C OMM AND>" + // 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("\n"), result, stashP[2]=="REVERSE"_hk ? F(" REVERSE") : F("")); diff --git a/DCCQueue.cpp b/DCCQueue.cpp index 8330e74..41dfddf 100644 --- a/DCCQueue.cpp +++ b/DCCQueue.cpp @@ -18,7 +18,13 @@ * along with CommandStation. If not, see . */ - +/* 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) { - lowPriorityQueue->addQueue(getSlot(NORMAL_PACKET,packet,length,repeats,0)); + void DCCQueue::scheduleDCCPacket(byte* packet, byte length, byte repeats, uint16_t loco) { + 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,48 +131,67 @@ PendingSlot* DCCQueue::recycleList=nullptr; }; - // Obtain packet (fills packet, length and repeats) - // returns 0 length if nothing in queue. + // Schedule the next dcc packet from the queues or an idle packet if none pending. + const byte idlePacket[] = {0xFF, 0x00}; - bool DCCQueue::scheduleNext() { - // check high priority queue first - if (!DCCWaveform::mainTrack.isReminderWindowOpen()) return false; - PendingSlot* previous=nullptr; - for (auto p=highPriorityQueue->head;p;p=p->next) { - // skip over pending ACC_OFF packets which are still delayed - if (p->type == ACC_OFF_PACKET && millis()startTime) continue; - // use this slot - DCCWaveform::mainTrack.schedulePacket(p->packet,p->packetLength,p->packetRepeat); - // 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); + bool DCCQueue::scheduleNext(bool force) { + if (highPriorityQueue->scheduleNextInternal()) return true; + if (lowPriorityQueue->scheduleNextInternal()) return true; + if (force) { + // This will arise when there is nothing available to be sent that will not compromise the rules + // typically this will only happen when there is only one loco in the reminders as the closely queued + // speed and function reminders must be separated by at least one packet not sent to that loco. + DCCWaveform::mainTrack.schedulePacket(idlePacket,sizeof(idlePacket),0); + lastSentPacketLocoId=0; return true; } - - // No high priopity packets found, check low priority queue - auto p=lowPriorityQueue->head; - if (!p) return false; // nothing in queues - - // schedule first packet in queue - DCCWaveform::mainTrack.schedulePacket(p->packet,p->packetLength,p->packetRepeat); + return false; + } - // remove from queue - lowPriorityQueue->head=p->next; - if (!lowPriorityQueue->head) lowPriorityQueue->tail=nullptr; - - if (p->type == ACC_ON_PACKET) { + bool DCCQueue::scheduleNextInternal() { + PendingSlot* previous=nullptr; + for (auto p=head;p;previous=p,p=p->next) { + // skip over pending ACC_OFF packets which are still delayed + if (p->type == ACC_OFF_PACKET && millis()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 this slot from the queue + if (previous) previous->next=p->next; + 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; + return true; + } + + // Recycle packet just consumed + recycle(p); + return true; + } + + // No packets found + return false; } // obtain and initialise slot for a PendingSlot. @@ -170,13 +202,30 @@ PendingSlot* DCCQueue::recycleList=nullptr; recycleList=p->next; } else { - DIAG(F("New DCC queue slot")); - p=new PendingSlot; // need a queue entry + 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; diff --git a/DCCQueue.h b/DCCQueue.h index 4c1eacb..ca1605a 100644 --- a/DCCQueue.h +++ b/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 - // so they can be easily discarded if an estop jumps the queue. + uint16_t locoId; // SPEED & FUNCTION packets 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. - // returns true if a packet was scheduled. - static bool scheduleNext(); + // Schedules a main track packet from the queues. + static bool scheduleNext(bool force); 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();