diff --git a/CommandDistributor.cpp b/CommandDistributor.cpp index 8205908..4c6fa46 100644 --- a/CommandDistributor.cpp +++ b/CommandDistributor.cpp @@ -207,9 +207,13 @@ int16_t CommandDistributor::retClockTime() { return lastclocktime; } -void CommandDistributor::broadcastLoco(byte slot) { - DCC::LOCO * sp=&DCC::speedTable[slot]; - broadcastReply(COMMAND_TYPE, F("\n"), sp->loco,slot,sp->targetSpeed,sp->functions); +void CommandDistributor::broadcastLoco(DCC::LOCO* sp) { + if (!sp) { + broadcastReply(COMMAND_TYPE,F("\n")); + return; + } + broadcastReply(COMMAND_TYPE, F("\n"), + sp->loco,sp->targetSpeed,sp->functions); #ifdef SABERTOOTH if (Serial2 && sp->loco == SABERTOOTH) { static uint8_t rampingmode = 0; diff --git a/CommandDistributor.h b/CommandDistributor.h index d86b87f..c2e600d 100644 --- a/CommandDistributor.h +++ b/CommandDistributor.h @@ -28,6 +28,7 @@ #include "StringBuffer.h" #include "defines.h" #include "EXRAIL2.h" +#include "DCC.h" #if WIFI_ON | ETHERNET_ON // Command Distributor must handle a RingStream of clients @@ -46,7 +47,7 @@ private: #endif public : static void parse(byte clientId,byte* buffer, RingStream * ring); - static void broadcastLoco(byte slot); + static void broadcastLoco(DCC::LOCO * slot); static void broadcastForgetLoco(int16_t loco); static void broadcastSensor(int16_t id, bool value); static void broadcastTurnout(int16_t id, bool isClosed); diff --git a/DCC.cpp b/DCC.cpp index a930a24..9bbd626 100644 --- a/DCC.cpp +++ b/DCC.cpp @@ -60,6 +60,8 @@ const byte FN_GROUP_5=0x10; FSH* DCC::shieldName=NULL; byte DCC::globalSpeedsteps=128; +#define SLOTLOOP for (auto slot=&speedTable[0];slot!=&speedTable[MAX_LOCOS];slot++) + void DCC::begin() { StringFormatter::send(&USB_SERIAL,F("\n"), F(VERSION), F(ARDUINO_TYPE), shieldName, F(GITHUB_SHA)); #ifndef DISABLE_EEPROM @@ -80,21 +82,21 @@ void DCC::setThrottle( uint16_t cab, uint8_t tSpeed, bool tDirection) { return; } byte speedCode = (tSpeed & 0x7F) + tDirection * 128; - int reg=lookupSpeedTable(cab); - if (reg<0 || speedTable[reg].targetSpeed==speedCode) return; - speedTable[reg].targetSpeed=speedCode; - auto momentum=speedTable[reg].millis_per_notch; + LOCO * slot=lookupSpeedTable(cab); + if (slot->targetSpeed==speedCode) return; + slot->targetSpeed=speedCode; + auto momentum=slot->millis_per_notch; if (momentum<0) momentum=defaultMomentum; if (momentum>0 && tSpeed!=1) { // not ESTOP // we dont throttle speed, we just let the reminders take it to target - speedTable[reg].momentum_base=millis(); + slot->momentum_base=millis(); } else { // Momentum not involved, throttle now. - speedTable[reg].speedCode = speedCode; + slot->speedCode = speedCode; setThrottle2(cab, speedCode); TrackManager::setDCSignal(cab,speedCode); // in case this is a dcc track on this addr } - CommandDistributor::broadcastLoco(reg); + CommandDistributor::broadcastLoco(slot); } void DCC::setThrottle2( uint16_t cab, byte speedCode) { @@ -162,19 +164,15 @@ int8_t DCC::getThrottleSpeed(int cab) { // or 128 (speed 0, dir forward) on "loco not found". // This is the throttle set speed uint8_t DCC::getThrottleSpeedByte(int cab) { - int reg=lookupSpeedTable(cab); - if (reg<0) - return 128; - return speedTable[reg].targetSpeed; + LOCO * slot=lookupSpeedTable(cab,false); + return slot?slot->targetSpeed:128; } // returns speed code byte for loco. // This is the most recently send DCC speed packet byte // or 128 (speed 0, dir forward) on "loco not found". uint8_t DCC::getLocoSpeedByte(int cab) { - int reg=lookupSpeedTable(cab); - if (reg<0) - return 128; - return speedTable[reg].speedCode; + LOCO* slot=lookupSpeedTable(cab,false); + return slot?slot->speedCode:128; } // returns 0 to 7 for frequency @@ -183,12 +181,11 @@ uint8_t DCC::getThrottleFrequency(int cab) { (void)cab; return 0; #else - int reg=lookupSpeedTable(cab); - if (reg<0) - return 0; // use default frequency + LOCO* slot=lookupSpeedTable(cab); + if (!slot) return 0; // use default frequency // shift out first 29 bits so we have the 3 "frequency bits" left - uint8_t res = (uint8_t)(speedTable[reg].functions >>29); - //DIAG(F("Speed table %d functions %l shifted %d"), reg, speedTable[reg].functions, res); + uint8_t res = (uint8_t)(slot->functions >>29); + //DIAG(F("Speed table %d functions %l shifted %d"), reg, slot->functions, res); return res; #endif } @@ -229,22 +226,21 @@ bool DCC::setFn( int cab, int16_t functionNumber, bool on) { if (functionNumber > 31) return true; - int reg = lookupSpeedTable(cab); - if (reg<0) return false; - + LOCO * slot = lookupSpeedTable(cab); + // Take care of functions: // Set state of function - uint32_t previous=speedTable[reg].functions; + uint32_t previous=slot->functions; uint32_t funcmask = (1UL<functions |= funcmask; } else { - speedTable[reg].functions &= ~funcmask; + slot->functions &= ~funcmask; } - if (speedTable[reg].functions != previous) { + if (slot->functions != previous) { if (functionNumber <= 28) - updateGroupflags(speedTable[reg].groupFlags, functionNumber); - CommandDistributor::broadcastLoco(reg); + updateGroupflags(slot->groupFlags, functionNumber); + CommandDistributor::broadcastLoco(slot); } return true; } @@ -252,14 +248,13 @@ bool DCC::setFn( int cab, int16_t functionNumber, bool on) { // Flip function state (used from withrottle protocol) void DCC::changeFn( int cab, int16_t functionNumber) { if (cab<=0 || functionNumber>31) return; - int reg = lookupSpeedTable(cab); - if (reg<0) return; + auto slot=lookupSpeedTable(cab); unsigned long funcmask = (1UL<functions ^= funcmask; if (functionNumber <= 28) { - updateGroupflags(speedTable[reg].groupFlags, functionNumber); + updateGroupflags(slot->groupFlags, functionNumber); } - CommandDistributor::broadcastLoco(reg); + CommandDistributor::broadcastLoco(slot); } // Report function state (used from withrottle protocol) @@ -267,12 +262,10 @@ void DCC::changeFn( int cab, int16_t functionNumber) { int8_t DCC::getFn( int cab, int16_t functionNumber) { if (cab<=0 || functionNumber>31) return -1; // unknown - int reg = lookupSpeedTable(cab); - if (reg<0) - return -1; - + auto slot = lookupSpeedTable(cab); + unsigned long funcmask = (1UL<functions & funcmask)? 1 : 0; } // Set the group flag to say we have touched the particular group. @@ -289,22 +282,22 @@ void DCC::updateGroupflags(byte & flags, int16_t functionNumber) { uint32_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; + auto slot = lookupSpeedTable(cab,false); + return slot?slot->functions:0; } // saves DC frequency (0..3) in spare functions 29,30,31 void DCC::setDCFreq(int cab,byte freq) { if (cab==0 || freq>3) return; - auto reg=lookupSpeedTable(cab,true); + auto slot=lookupSpeedTable(cab,true); // drop and replace F29,30,31 (top 3 bits) - auto newFunctions=speedTable[reg].functions & 0x1FFFFFFFUL; + auto newFunctions=slot->functions & 0x1FFFFFFFUL; if (freq==1) newFunctions |= (1UL<<29); // F29 else if (freq==2) newFunctions |= (1UL<<30); // F30 else if (freq==3) newFunctions |= (1UL<<31); // F31 - if (newFunctions==speedTable[reg].functions) return; // no change - speedTable[reg].functions=newFunctions; - CommandDistributor::broadcastLoco(reg); + if (newFunctions==slot->functions) return; // no change + slot->functions=newFunctions; + CommandDistributor::broadcastLoco(slot); } void DCC::setAccessory(int address, byte port, bool gate, byte onoff /*= 2*/) { @@ -760,9 +753,9 @@ void DCC::setConsistId(int id,bool reverse,ACK_CALLBACK callback) { void DCC::forgetLoco(int cab) { // removes any speed reminders for this loco setThrottle2(cab,1); // ESTOP this loco if still on track - int reg=lookupSpeedTable(cab, false); - if (reg>=0) { - speedTable[reg].loco=0; + auto slot=lookupSpeedTable(cab, false); + if (slot) { + slot->loco=-1; // no longer used but not end of world CommandDistributor::broadcastForgetLoco(cab); } } @@ -770,7 +763,7 @@ void DCC::forgetAllLocos() { // removes all speed reminders setThrottle2(0,1); // ESTOP all locos still on track for (int i=0;i highestUsedReg) reg = 0; // Go to start of table - if (speedTable[reg].loco > 0) { - // have found loco to remind - if (issueReminder(reg)) - lastLocoReminder = reg; - } else - lastLocoReminder = reg; + auto slot = nextLocoReminder; + if (slot >= &speedTable[MAX_LOCOS]) slot=&speedTable[0]; // Go to start of table + if (slot->loco > 0) + if (!issueReminder(slot)) + return; + // a loco=0 is at the end of the list, a loco <0 is deleted + if (slot->loco==0) nextLocoReminder = &speedTable[0]; + else nextLocoReminder=slot+1; } int16_t normalize(byte speed) { @@ -807,27 +800,27 @@ byte dccalize(int16_t speed) { return (int16_t)-1 - speed; } -bool DCC::issueReminder(int reg) { - unsigned long functions=speedTable[reg].functions; - int loco=speedTable[reg].loco; - byte flags=speedTable[reg].groupFlags; +bool DCC::issueReminder(LOCO * slot) { + unsigned long functions=slot->functions; + int loco=slot->loco; + byte flags=slot->groupFlags; switch (loopStatus) { case 0: { // calculate any momentum change going on - auto sc=speedTable[reg].speedCode; - if (speedTable[reg].targetSpeed!=sc) { + auto sc=slot->speedCode; + if (slot->targetSpeed!=sc) { // calculate new speed code auto now=millis(); - int16_t delay=now-speedTable[reg].momentum_base; - auto millisPerNotch=speedTable[reg].millis_per_notch; + int16_t delay=now-slot->momentum_base; + auto millisPerNotch=slot->millis_per_notch; if (millisPerNotch<0) millisPerNotch=defaultMomentum; // allow for momentum change to 0 while accelerating/slowing auto ticks=(millisPerNotch>0)?(delay/millisPerNotch):500; if (ticks>0) { auto current=normalize(sc); // -128..+127 - auto target=normalize(speedTable[reg].targetSpeed); - // DIAG(F("Momentum l=%d t=%d sc=%d c=%d t=%d"),loco,ticks,sc,current,target); + auto target=normalize(slot->targetSpeed); + // DIAG(F("Momentum l=%d ti=%d sc=%d c=%d t=%d"),loco,ticks,sc,current,target); if (currenttarget) current=target; @@ -837,13 +830,13 @@ bool DCC::issueReminder(int reg) { if (currentspeedCode=sc; TrackManager::setDCSignal(loco,sc); // in case this is a dcc track on this addr - speedTable[reg].momentum_base=now; + slot->momentum_base=now; } } - // DIAG(F("Reminder %d speed %d"),loco,speedTable[reg].speedCode); + // DIAG(F("Reminder %d speed %d"),loco,slot->speedCode); setThrottle2(loco, sc); } break; @@ -907,32 +900,28 @@ byte DCC::cv2(int cv) { return lowByte(cv); } -int DCC::lookupSpeedTable(int locoId, bool autoCreate) { +DCC::LOCO * DCC::lookupSpeedTable(int locoId, bool autoCreate) { // determine speed reg for this loco - int firstEmpty = MAX_LOCOS; - int reg; - for (reg = 0; reg < MAX_LOCOS; reg++) { - if (speedTable[reg].loco == locoId) break; - if (speedTable[reg].loco == 0 && firstEmpty == MAX_LOCOS) firstEmpty = reg; + LOCO * firstEmpty=nullptr; + SLOTLOOP { + if (firstEmpty==nullptr && slot->loco<=0) firstEmpty=slot; + if (slot->loco == locoId) return slot; + if (slot->loco==0) break; } - - // return -1 if not found and not auto creating - if (reg== MAX_LOCOS && !autoCreate) return -1; - if (reg == MAX_LOCOS) reg = firstEmpty; - if (reg >= MAX_LOCOS) { - DIAG(F("Too many locos")); - return -1; + if (!autoCreate) return nullptr; + if (firstEmpty==nullptr) { + // return last slot if full + DIAG(F("Too many locos, reusing last slot")); + firstEmpty=&speedTable[MAX_LOCOS-1]; } - if (reg==firstEmpty){ - speedTable[reg].loco = locoId; - speedTable[reg].speedCode=128; // default direction forward - speedTable[reg].targetSpeed=128; // default direction forward - speedTable[reg].groupFlags=0; - speedTable[reg].functions=0; - speedTable[reg].millis_per_notch=-1; // use default - } - if (reg > highestUsedReg) highestUsedReg = reg; - return reg; + // fill first empty slot with new entry + firstEmpty->loco = locoId; + firstEmpty->speedCode=128; // default direction forward + firstEmpty->targetSpeed=128; // default direction forward + firstEmpty->groupFlags=0; + firstEmpty->functions=0; + firstEmpty->millis_per_notch=-1; // use default + return firstEmpty; } bool DCC::setMomentum(int locoId,int16_t millis_per_notch) { @@ -944,44 +933,42 @@ bool DCC::setMomentum(int locoId,int16_t millis_per_notch) { // We dont copy the default here because it can be changed // while running and have immediate effect on all locos using -1. if (locoId<=0 || millis_per_notch<-1) return false; - auto reg=lookupSpeedTable(locoId); - if (reg<0) return false; // table full - speedTable[reg].millis_per_notch=millis_per_notch; + lookupSpeedTable(locoId,true)->millis_per_notch=millis_per_notch; return true; } - + + void DCC::estopAll() { setThrottle2(0,1); // estop all locos TrackManager::setDCSignal(0,1); // remind stop/estop but dont change direction - for (int reg = 0; reg <= highestUsedReg; reg++) { - if (speedTable[reg].loco==0) continue; - byte newspeed=(speedTable[reg].speedCode & 0x80) | 0x01; - speedTable[reg].speedCode = newspeed; - speedTable[reg].targetSpeed = newspeed; - CommandDistributor::broadcastLoco(reg); + SLOTLOOP { + if (slot->loco<=0) continue; + byte newspeed=(slot->targetSpeed & 0x80) | 0x01; + slot->speedCode = newspeed; + slot->targetSpeed = newspeed; + CommandDistributor::broadcastLoco(slot); } } DCC::LOCO DCC::speedTable[MAX_LOCOS]; -int DCC::lastLocoReminder = 0; -int DCC::highestUsedReg = 0; +DCC::LOCO * DCC::nextLocoReminder = &DCC::speedTable[0]; void DCC::displayCabList(Print * stream) { - + StringFormatter::send(stream,F("<*\n")); int used=0; - for (int reg = 0; reg <= highestUsedReg; reg++) { - if (speedTable[reg].loco>0) { + SLOTLOOP { + if (slot->loco==0) break; // no more locos + if (slot->loco>0) { used ++; - StringFormatter::send(stream,F("cab=%d, speed=%d, dir=%c momentum=%d\n"), - speedTable[reg].loco, speedTable[reg].speedCode & 0x7f, - (speedTable[reg].speedCode & 0x80) ? 'F':'R', - speedTable[reg].millis_per_notch); + StringFormatter::send(stream,F("cab=%d, speed=%d, target=%d momentum=%d\n"), + slot->loco, slot->speedCode, slot->targetSpeed, + slot->millis_per_notch); } } - StringFormatter::send(stream,F("Used=%d, max=%d\n"),used,MAX_LOCOS); - + StringFormatter::send(stream,F("Used=%d, max=%d, momentum=%d *>\n"), + used,MAX_LOCOS, DCC::defaultMomentum); } diff --git a/DCC.h b/DCC.h index 2ee66e0..faa43f7 100644 --- a/DCC.h +++ b/DCC.h @@ -110,7 +110,7 @@ public: byte targetSpeed; // speed set by throttle }; static LOCO speedTable[MAX_LOCOS]; - static int lookupSpeedTable(int locoId, bool autoCreate=true); + static LOCO * lookupSpeedTable(int locoId, bool autoCreate=true); static byte cv1(byte opcode, int cv); static byte cv2(int cv); static bool setMomentum(int locoId,int16_t millis_per_notch); @@ -120,9 +120,8 @@ private: static int16_t defaultMomentum; // Millis per speed step static void setThrottle2(uint16_t cab, uint8_t speedCode); static void setFunctionInternal(int cab, byte fByte, byte eByte, byte count); - static bool issueReminder(int reg); - static int lastLocoReminder; - static int highestUsedReg; + static bool issueReminder(LOCO * slot); + static LOCO* nextLocoReminder; static FSH *shieldName; static byte globalSpeedsteps; diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp index 4fa9d86..27462fe 100644 --- a/DCCEXParser.cpp +++ b/DCCEXParser.cpp @@ -289,12 +289,9 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream) int16_t direction; if (params==1) { // display state - int16_t slot=DCC::lookupSpeedTable(p[0],false); - if (slot>=0) - CommandDistributor::broadcastLoco(slot); - else // send dummy state speed 0 fwd no functions. - StringFormatter::send(stream,F("\n"),p[0]); - return; + if (p[0]<=0) break; + CommandDistributor::broadcastLoco(DCC::lookupSpeedTable(p[0],false)); + return; } if (params == 4)