From f41f61dd5fc1adb8e0445f8fbfb4c3f8011d089a Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Sat, 6 Apr 2024 23:41:25 +0100
Subject: [PATCH] <W CONSIST cmd

---
 DCC.cpp         | 33 +++++++++++++++++++++++++++++++++
 DCC.h           |  2 +-
 DCCEXParser.cpp | 11 +++++++++++
 DCCEXParser.h   |  1 +
 4 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/DCC.cpp b/DCC.cpp
index f2ab8c3..2ed75f1 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -615,6 +615,19 @@ const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
       CALLFAIL
 };
 
+// for CONSIST_ID_PROG the 20,19 values are already calculated
+const ackOp FLASH CONSIST_ID_PROG[] = {
+      BASELINE,
+      SETCV,(ackOp)20,
+      SETBYTEH,    // high byte to CV 20
+      WB,WACK,     // ignore dedcoder without cv20 support
+      SETCV,(ackOp)19,
+      SETBYTEL,   // low byte of word
+      WB,WACK,ITC1,   // If ACK, we are done - callback(1) means Ok
+      VB,WACK,ITC1,   // Some decoders do not ack and need verify
+      CALLFAIL
+};
+
 const ackOp FLASH LONG_LOCO_ID_PROG[] = {
       BASELINE,
       // Clear consist CV 19,20
@@ -689,6 +702,26 @@ void DCC::setLocoId(int id,ACK_CALLBACK callback) {
       DCCACK::Setup(id | 0xc000,LONG_LOCO_ID_PROG, callback);
 }
 
+void DCC::setConsistId(int id,bool reverse,ACK_CALLBACK callback) {
+  if (id<1 || id>10239) { //0x27FF according to standard
+    callback(-1);
+    return;
+  }
+  byte cv20;
+  byte cv19;
+
+  if (id<=HIGHEST_SHORT_ADDR) {
+    cv19=id;
+    cv20=0;
+  }
+  else {
+    cv20=id/100;
+    cv19=id%100;
+  }
+  if (reverse) cv19|=0x80;
+  DCCACK::Setup((cv20<<8)|cv19, CONSIST_ID_PROG, 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);
diff --git a/DCC.h b/DCC.h
index 4503227..4bc222c 100644
--- a/DCC.h
+++ b/DCC.h
@@ -85,7 +85,7 @@ public:
 
   static void getLocoId(ACK_CALLBACK callback);
   static void setLocoId(int id,ACK_CALLBACK callback);
-
+  static void setConsistId(int id,bool reverse,ACK_CALLBACK callback);
   // Enhanced API functions
   static void forgetLoco(int cab); // removes any speed reminders for this loco
   static void forgetAllLocos();    // removes all speed reminders
diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp
index fa4c9f8..6e41473 100644
--- a/DCCEXParser.cpp
+++ b/DCCEXParser.cpp
@@ -458,6 +458,9 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
             DCC::setLocoId(p[0],callback_Wloco);
         else if (params == 4)  // WRITE CV ON PROG <W CV VALUE [CALLBACKNUM] [CALLBACKSUB]>
             DCC::writeCVByte(p[0], p[1], callback_W4);
+        else if ((params==2 | params==3 ) && p[0]=="CONSIST"_hk ) {
+            DCC::setConsistId(p[1],p[2]=="REVERSE"_hk,callback_Wconsist);
+        }    
         else if (params == 2)  // WRITE CV ON PROG <W CV VALUE>
             DCC::writeCVByte(p[0], p[1], callback_W);
 	else
@@ -1347,3 +1350,11 @@ void DCCEXParser::callback_Wloco(int16_t result)
     StringFormatter::send(getAsyncReplyStream(), F("<w %d>\n"), result);
     commitAsyncReplyStream();
 }
+
+void DCCEXParser::callback_Wconsist(int16_t result)
+{
+    if (result==1) result=stashP[1]; // pick up original requested id from command
+    StringFormatter::send(getAsyncReplyStream(), F("<w CONSIST %d%S>\n"),
+     result, stashP[2]=="REVERSE"_hk ? F(" REVERSE") : F(""));
+    commitAsyncReplyStream();
+}
diff --git a/DCCEXParser.h b/DCCEXParser.h
index 3c3382c..d3b7851 100644
--- a/DCCEXParser.h
+++ b/DCCEXParser.h
@@ -71,6 +71,7 @@ struct DCCEXParser
     static void callback_R(int16_t result);
     static void callback_Rloco(int16_t result);
     static void callback_Wloco(int16_t result);
+    static void callback_Wconsist(int16_t result);
     static void callback_Vbit(int16_t result);
     static void callback_Vbyte(int16_t result);
     static FILTER_CALLBACK  filterCallback;