diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp index 1de8900..6e07f9a 100644 --- a/DCCEXParser.cpp +++ b/DCCEXParser.cpp @@ -346,7 +346,9 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream) break; if (params == 1) // Write new loco id (clearing consist and managing short/long) DCC::setLocoId(p[0],callback_Wloco); - else // WRITE CV ON PROG + else if (params == 4) // WRITE CV ON PROG + DCC::writeCVByte(p[0], p[1], callback_W4); + else // WRITE CV ON PROG DCC::writeCVByte(p[0], p[1], callback_W); return; @@ -945,7 +947,14 @@ void DCCEXParser::commitAsyncReplyStream() { void DCCEXParser::callback_W(int16_t result) { StringFormatter::send(getAsyncReplyStream(), - F("\n"), stashP[2], stashP[3], stashP[0], result == 1 ? stashP[1] : -1); + F("\n"), stashP[0], result == 1 ? stashP[1] : -1); + commitAsyncReplyStream(); +} + +void DCCEXParser::callback_W4(int16_t result) +{ + StringFormatter::send(getAsyncReplyStream(), + F("\n"), stashP[2], stashP[3], stashP[0], result == 1 ? stashP[1] : -1); commitAsyncReplyStream(); } diff --git a/DCCEXParser.h b/DCCEXParser.h index 227e3f0..d9fe0e1 100644 --- a/DCCEXParser.h +++ b/DCCEXParser.h @@ -60,6 +60,7 @@ struct DCCEXParser static int16_t stashP[MAX_COMMAND_PARAMS]; static bool stashCallback(Print * stream, int16_t p[MAX_COMMAND_PARAMS], RingStream * ringStream); static void callback_W(int16_t result); + static void callback_W4(int16_t result); static void callback_B(int16_t result); static void callback_R(int16_t result); static void callback_Rloco(int16_t result);