diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp index 98cd1be..e3c9eac 100644 --- a/DCCEXParser.cpp +++ b/DCCEXParser.cpp @@ -958,18 +958,20 @@ void DCCEXParser::callback_R(int16_t result) commitAsyncReplyStream(); } -void DCCEXParser::callback_Rloco(int16_t result) -{ - if (result <= 0) - StringFormatter::send(getAsyncReplyStream(), F("\n")); - else if (result & LONG_ADDR_MARKER ) { //long addr - result = result & ~LONG_ADDR_MARKER; - if (result > HIGHEST_SHORT_ADDR) //real long - StringFormatter::send(getAsyncReplyStream(), F("\n"), result); +void DCCEXParser::callback_Rloco(int16_t result) { + const FSH * detail; + if (result<=0) { + detail=F("\n"); + } else { + bool longAddr=result & LONG_ADDR_MARKER; //long addr + if (longAddr) + result = result^LONG_ADDR_MARKER; + if (longAddr && result <= HIGHEST_SHORT_ADDR) + detail=F("\n"); else - StringFormatter::send(getAsyncReplyStream(), F("\n"), result); - } else // short addr - StringFormatter::send(getAsyncReplyStream(), F("\n"), result); + detail=F("\n"); + } + StringFormatter::send(getAsyncReplyStream(), detail, result); commitAsyncReplyStream(); } diff --git a/WiThrottle.cpp b/WiThrottle.cpp index 62369ce..8aabb82 100644 --- a/WiThrottle.cpp +++ b/WiThrottle.cpp @@ -51,6 +51,8 @@ #include "version.h" #include "RMFT2.h" +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) #define LOOPLOCOS(THROTTLECHAR, CAB) for (int loco=0;locomark(stashClient); - char addrchar; - if (locoid & LONG_ADDR_MARKER) { // long addr - locoid = locoid & ~LONG_ADDR_MARKER; - addrchar = 'L'; - } else - addrchar = 'S'; - if (locoid<=0) StringFormatter::send(stashStream,F("HMNo loco found on prog track\n")); - else if (addrchar == 'L' && locoid <= HIGHEST_SHORT_ADDR ) - StringFormatter::send(stashStream,F("HMLong addr <= 127 not supported\n")); else { - char addcmd[20]={'M',stashThrottleChar,'+', addrchar}; - itoa(locoid,addcmd+4,10); - stashInstance->multithrottle(stashStream, (byte *)addcmd); - DCCWaveform::progTrack.setPowerMode(POWERMODE::ON); - DCC::setProgTrackSyncMain(true); // <1 JOIN> so we can drive loco away + // short or long + char addrchar; + if (locoid & LONG_ADDR_MARKER) { // long addr + locoid = locoid ^ LONG_ADDR_MARKER; + addrchar = 'L'; + } else + addrchar = 'S'; + if (addrchar == 'L' && locoid <= HIGHEST_SHORT_ADDR ) + StringFormatter::send(stashStream,F("HMLong addr <= " STR(HIGHEST_SHORT_ADDR) " not supported\n")); + else { + char addcmd[20]={'M',stashThrottleChar,'+', addrchar}; + itoa(locoid,addcmd+4,10); + stashInstance->multithrottle(stashStream, (byte *)addcmd); + DCCWaveform::progTrack.setPowerMode(POWERMODE::ON); + DCC::setProgTrackSyncMain(true); // <1 JOIN> so we can drive loco away + } } stashStream->commit(); }