diff --git a/DCC.cpp b/DCC.cpp index 9704639..2789a4d 100644 --- a/DCC.cpp +++ b/DCC.cpp @@ -162,7 +162,7 @@ void DCC::setThrottle2( uint16_t cab, byte speedCode) { else DCCQueue::scheduleDCCSpeedPacket( b, nB, 0, cab); } -void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) { +void DCC::setFunctionInternal(int cab, byte group, byte byte1, byte byte2) { // DIAG(F("setFunctionInternal %d %x %x"),cab,byte1,byte2); byte b[4]; byte nB = 0; @@ -172,8 +172,8 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) { b[nB++] = lowByte(cab); if (byte1!=0) b[nB++] = byte1; b[nB++] = byte2; - - DCCQueue::scheduleDCCPacket(b, nB, 0, cab); + + DCCQueue::scheduleDCCFunctionPacket(b, nB, cab,group); } // returns speed steps 0 to 127 (1 == emergency stop) @@ -949,31 +949,31 @@ bool DCC::issueReminder(LOCO * slot) { return true; // reminder sent case 1: // remind function group 1 (F0-F4) if (flags & FN_GROUP_1) { - setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4)); // 100D DDDD + setFunctionInternal(loco,1,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4)); // 100D DDDD return true; // reminder sent } break; case 3: // remind function group 2 F5-F8 if (flags & FN_GROUP_2) { - setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F)); // 1011 DDDD + setFunctionInternal(loco,2,0, 176 | ((functions>>5)& 0x0F)); // 1011 DDDD return true; // reminder sent } break; case 5: // remind function group 3 F9-F12 if (flags & FN_GROUP_3) { - setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F)); // 1010 DDDD + setFunctionInternal(loco,3,0, 160 | ((functions>>9)& 0x0F)); // 1010 DDDD return true; // reminder sent } break; case 7: // remind function group 4 F13-F20 if (flags & FN_GROUP_4) { - setFunctionInternal(loco,222, ((functions>>13)& 0xFF)); + setFunctionInternal(loco,4,222, ((functions>>13)& 0xFF)); return true; } break; case 9: // remind function group 5 F21-F28 if (flags & FN_GROUP_5) { - setFunctionInternal(loco,223, ((functions>>21)& 0xFF)); + setFunctionInternal(loco,5,223, ((functions>>21)& 0xFF)); return true; // reminder sent } break; diff --git a/DCC.h b/DCC.h index af8c21b..2ce0ae8 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); + static void setFunctionInternal(int cab, byte group, byte fByte, byte eByte); static bool issueReminder(LOCO * slot); static LOCO* nextLocoReminder; static FSH *shieldName; diff --git a/DCCQueue.cpp b/DCCQueue.cpp index 735ef40..e04fa4b 100644 --- a/DCCQueue.cpp +++ b/DCCQueue.cpp @@ -94,9 +94,35 @@ uint16_t DCCQueue::lastSentPacketLocoId=0; // used to prevent two packets to the return; } } - highPriorityQueue->addQueue(getSlot(NORMAL_PACKET,packet,length,repeats,loco)); + highPriorityQueue->addQueue(getSlot(SPEED_PACKET,packet,length,repeats,loco)); } + // Packet replaces existing loco function packet or joins end of high priority queue. + + void DCCQueue::scheduleDCCFunctionPacket(byte* packet, byte length, uint16_t loco, byte group) { + PendingType type=DEAD_PACKET; + switch(group) { + case 1: type=FUNCTION1_PACKET; break; + case 2: type=FUNCTION2_PACKET; break; + case 3: type=FUNCTION3_PACKET; break; + case 4: type=FUNCTION4_PACKET; break; + case 5: type=FUNCTION5_PACKET; break; + default: + DIAG(F("DCCQueue::scheduleDCCFunctionPacket invalid group %d"),group); + return; // invalid group + } + + for (auto p=lowPriorityQueue->head;p;p=p->next) { + if (p->locoId==loco && p->type==type) { + // replace existing packet for same loco and function group + memcpy(p->packet,packet,length); + p->packetLength=length; + p->packetRepeat=0; + return; + } + } + lowPriorityQueue->addQueue(getSlot(type,packet,length,0,loco)); + } // ESTOP - // any outstanding throttle packet for this loco (all if loco=0) discarded @@ -119,7 +145,7 @@ uint16_t DCCQueue::lastSentPacketLocoId=0; // used to prevent two packets to the } } // add the estop packet to the start of the queue - highPriorityQueue->jumpQueue(getSlot(NORMAL_PACKET,packet,length,repeats,0)); + highPriorityQueue->jumpQueue(getSlot(SPEED_PACKET,packet,length,repeats,0)); } // Accessory coil-On Packet joins end of queue as normal. diff --git a/DCCQueue.h b/DCCQueue.h index c3c8abc..80e72b9 100644 --- a/DCCQueue.h +++ b/DCCQueue.h @@ -23,7 +23,10 @@ #include "Arduino.h" #include "DCCWaveform.h" -enum PendingType:byte {NORMAL_PACKET,SPEED_PACKET,FUNCTION_PACKET,ACC_ON_PACKET,ACC_OFF_PACKET,DEAD_PACKET}; +enum PendingType:byte {NORMAL_PACKET, + FUNCTION1_PACKET, FUNCTION2_PACKET, FUNCTION3_PACKET, FUNCTION4_PACKET, FUNCTION5_PACKET, + SPEED_PACKET,ACC_ON_PACKET,ACC_OFF_PACKET,DEAD_PACKET}; + struct PendingSlot { PendingSlot* next; PendingType type; @@ -40,14 +43,16 @@ enum PendingType:byte {NORMAL_PACKET,SPEED_PACKET,FUNCTION_PACKET,ACC_ON_PACKET, class DCCQueue { public: - - + // Non-speed packets are queued in the main queue 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); + // Function group packets are queued in the low priority queue + static void scheduleDCCFunctionPacket(byte* packet, byte length, uint16_t loco, byte group); + // ESTOP packets jump the high priority queue and discard any outstanding throttle packets for this loco static void scheduleEstopPacket(byte* packet, byte length, byte repeats,uint16_t loco);