copy in

CircularBuffer.cpp

@@ -0,0 +1,387 @@
+/*
+ *  © 2023 Thierry Paris / Locoduino
+ *  All rights reserved.
+ *  
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "CircularBuffer.hpp"
+
+CircularBuffer::CircularBuffer(int inSize)
+{
+	this->head = 0;
+	this->tail = 0;
+	this->full = false;
+	this->peakCount = 0;
+	this->buffer = NULL;
+	this->size = inSize;
+#ifdef ARDUINO_ARCH_ESP32
+	this->xSemaphore = NULL;
+#endif
+}
+
+void CircularBuffer::begin(bool inMultiThread)
+{
+#ifdef ARDUINO_ARCH_ESP32
+	if (inMultiThread)
+	{
+		this->xSemaphore = xSemaphoreCreateMutex();
+	}
+#endif
+	this->buffer = new byte[this->size];
+	this->clear();
+}
+
+bool CircularBuffer::CheckIfBeginDone()
+{
+	if (this->buffer == NULL)
+	{
+		DIAG(F("Error : CircularBuffer missing begin !"));
+		return false;
+	}
+	return true;
+}
+
+void CircularBuffer::end()
+{
+	START_SEMAPHORE()
+	if (!this->CheckIfBeginDone()) return;
+
+	delete[] this->buffer;
+	this->buffer = NULL;	// to be sure to crash at any attemps to use it !
+	END_SEMAPHORE()
+}
+
+void CircularBuffer::clear()
+{
+	if (!this->CheckIfBeginDone()) return;
+
+	START_SEMAPHORE()
+	memset(this->buffer, 0, this->size);
+
+	this->full = false;
+	this->head = this->tail = 0;
+	this->peakCount = 0;
+	END_SEMAPHORE()
+}
+
+bool CircularBuffer::PushByte(byte inpData)
+{
+	if (!this->CheckIfBeginDone()) return false;
+
+	bool ok = true;
+	START_SEMAPHORE()
+
+	if (!this->full)
+	{
+		this->buffer[this->head] = inpData;
+		this->head++;
+		this->full = this->head == this->tail;
+
+		if (this->full)
+		{
+			ok = false;
+			this->peakCount = this->size + 1;	// maximum size !
+		}
+	}
+
+	if (!ok)
+	{
+		DIAG(F("Error : the byte has been lost ! Buffer is full !"));
+	}
+
+	END_SEMAPHORE()
+
+	this->GetCount();	// update peakCount...
+	return ok;
+}
+
+bool CircularBuffer::PushBytes(byte* inpData, int inDataLength)
+{
+	if (!this->CheckIfBeginDone()) return false;
+
+	bool ok = true;
+	START_SEMAPHORE()
+	if (!this->full)
+	{
+		for (int i = 0; i < inDataLength; i++)
+		{
+			this->buffer[this->head] = inpData[i];
+			this->head = (this->head + 1) % this->size;
+			this->full = this->head == this->tail;
+
+			if (this->full)
+			{
+				ok = false;
+				this->peakCount = this->size + (inDataLength - i);	// maximum size !
+				break;
+			}
+		}
+	}
+
+	if (!ok)
+	{
+		DIAG(F("Error : bytes has been lost ! Buffer is full !"));
+	}
+
+	END_SEMAPHORE()
+	this->GetCount();	// update peakCount...
+	return ok;
+}
+
+byte CircularBuffer::GetByte()
+{
+	if (!this->CheckIfBeginDone()) return 0;
+
+	byte value = 0;
+	if (this->isEmpty())
+		return 0;
+
+	START_SEMAPHORE()
+	value = this->buffer[this->tail];
+	this->tail = (this->tail + 1) % this->size;
+	this->full = false;
+	END_SEMAPHORE()
+
+	return value;
+}
+
+int16_t CircularBuffer::GetInt16()
+{
+	if (!this->CheckIfBeginDone()) return 0;
+
+	if (this->isEmpty() || this->GetCount() < 2)
+		return 0;
+
+	byte value1 = this->GetByte();
+	byte value2 = this->GetByte();
+
+	return int16_t((unsigned char)(value2) << 8 | (unsigned char)(value1));
+}
+
+int32_t CircularBuffer::GetInt32()
+{
+	if (!this->CheckIfBeginDone()) return 0;
+
+	if (this->isEmpty() || this->GetCount() < 4)
+		return 0;
+
+	byte value1 = this->GetByte();
+	byte value2 = this->GetByte();
+	byte value3 = this->GetByte();
+	byte value4 = this->GetByte();
+
+	return int32_t(value4 << 24 | value3 << 16 | value2 << 8 | value1);
+}
+
+bool CircularBuffer::GetBytes(byte* inpData, int inDataLength)
+{
+	if (!this->CheckIfBeginDone()) return false;
+
+	if (this->GetCount() < inDataLength)
+		return false;
+
+	START_SEMAPHORE()
+	for (int i = 0; i < inDataLength; i++)
+	{
+		inpData[i] = this->buffer[this->tail];
+		this->tail = (this->tail + 1) % this->size;
+	}
+	this->full = false;
+	END_SEMAPHORE()
+
+	return true;
+}
+
+int16_t CircularBuffer::GetInt16(byte* pBuffer, int inPos)
+{
+	byte value1 = pBuffer[inPos];
+	byte value2 = pBuffer[inPos+1];
+
+	return int16_t((unsigned char)(value2) << 8 | (unsigned char)(value1));
+}
+
+int32_t CircularBuffer::GetInt32(byte* pBuffer, int inPos)
+{
+	byte value1 = pBuffer[inPos];
+	byte value2 = pBuffer[inPos + 1];
+	byte value3 = pBuffer[inPos + 2];
+	byte value4 = pBuffer[inPos + 3];
+
+	return int32_t(value4 << 24 | value3 << 16 | value2 << 8 | value1);
+}
+
+void CircularBuffer::GetBytes(byte* pBuffer, int inPos, byte* inpData, int inDataLength)
+{
+	memcpy(pBuffer + inPos, inpData, inDataLength);
+}
+
+int CircularBuffer::GetCount()
+{
+	if (!this->CheckIfBeginDone()) return 0;
+
+	int usedSize = 0;
+
+	usedSize = this->size;
+
+	if (!this->full)
+	{
+		if (this->head >= this->tail)
+		{
+			usedSize = this->head - this->tail;
+		}
+		else
+		{
+			usedSize = this->size + this->head - this->tail;
+		}
+	}
+
+	if (usedSize > this->peakCount)
+		this->peakCount = usedSize;
+
+	return usedSize;
+}
+
+#ifdef DCCPP_DEBUG_MODE
+#ifdef VISUALSTUDIO
+const char textNum[] = "123456789";
+const char textChars[] = "ABCDEF";
+const char textSymb[] = "/*-+&$!:;,";
+void CircularBuffer::Test()
+{
+	CircularBuffer test(20);
+
+	test.begin(false);
+
+	Serial.println("After all initialized");
+	test.printCircularBuffer();
+
+	test.PushBytes((byte*)textNum, 9);
+	Serial.println("After nums pushed");
+	test.printCircularBuffer();
+
+	byte ret = test.GetByte();
+	Serial.print("ret '1' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '2': ");
+	Serial.println((int)ret);
+	test.printCircularBuffer();
+
+	test.PushBytes((byte*)textChars, 6);
+	Serial.println("After chars pushed");
+	test.printCircularBuffer();
+
+	ret = test.GetByte();
+	Serial.print("ret '3' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '4': ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '5': ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '6': ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '7': ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '8': ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '9': ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret 'A': ");
+	Serial.println((int)ret);
+	test.printCircularBuffer();
+
+	test.PushBytes((byte*)textSymb, 10);
+	Serial.println("After chars pushed");
+	test.printCircularBuffer();
+
+	ret = test.GetByte();
+	Serial.print("ret 'B' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret 'C' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret 'D' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret 'E' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret 'F' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '/' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '*' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '-' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '+' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '&' : ");
+	Serial.println((int)ret);
+	ret = test.GetByte();
+	Serial.print("ret '$' : ");
+	Serial.println((int)ret);
+	test.printCircularBuffer();
+
+	Serial.print("Final size : ");
+	Serial.println((int)test.GetCount());
+	Serial.print("Max used : ");
+	Serial.println((int)test.GetPeakCount());
+
+	// add too much data in the buffer...
+	test.PushBytes((byte*)textSymb, 10);
+	Serial.println("After chars pushed");
+	test.PushBytes((byte*)textNum, 9);
+	Serial.println("After chars pushed");
+	test.printCircularBuffer();
+
+	Serial.print("Final size : ");
+	Serial.println((int)test.GetCount());
+	Serial.print("Max used : ");
+	Serial.println((int)test.GetPeakCount());
+}
+#endif
+
+void CircularBuffer::printCircularBuffer()
+{
+	if (!this->CheckIfBeginDone()) return;
+
+	if (this->full)
+		Serial.println("FULL !");
+	for (int i = 0; i < this->size; i++)
+	{
+		if (i == this->tail)
+			Serial.print("Tail ");
+		if (i == this->head)
+			Serial.print("Head ");
+		if (i != this->tail && i != this->head)
+			Serial.print("     ");
+		Serial.println((int)this->buffer[i]);
+	}
+}
+#endif

CircularBuffer.hpp

@@ -0,0 +1,185 @@
+/*
+ *  © 2023 Thierry Paris / Locoduino
+ *  All rights reserved.
+ *  
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+//-------------------------------------------------------------------
+#ifndef __CircularBuffer_Hpp__
+#define __CircularBuffer_Hpp__
+//-------------------------------------------------------------------
+
+#include <Arduino.h>
+#include "DIAG.h"
+
+/** This is a thread-safe buffer of bytes, binary or not. Bytes are pushed on the top (its head), and got from the bottom of the 
+* buffer (its tail...).
+*/
+class CircularBuffer
+{
+private:
+	byte *buffer;	// buffer itself
+	int size;			// total size of this buffer
+	int head;			// index of the first free byte in the buffer
+	int tail;			// index of the next byte to get.
+	bool full;		// if true, no more space !
+	int peakCount;	// biggest occupancy size since the creation of the buffer.
+#ifdef ARDUINO_ARCH_ESP32
+	SemaphoreHandle_t xSemaphore; // semaphore d'exclusion mutuelle
+#endif
+
+public:
+	/** Constructor.
+	@param inSize	
+	*/
+	CircularBuffer(int inSize);
+
+	/** Initialize the list.
+	@param inMultiThread	If the buffer is used in multi-thread environnement, initialize the semaphore before calling this begin().
+	*/
+	void begin(bool inMultiThread = false);
+
+	/** Close the usage of this buffer and free the allocated memory.
+	*/
+	void end();
+
+	/** Remove the full content of the buffer, ready for the next push.
+	*/
+	void clear();
+
+	/** Add one byte in the buffer.
+	@param inpData	byte to add.
+	@return true if the byte have been pushed. false if the byte are lost due to max size reached...
+	*/
+	bool PushByte(byte inpData);
+
+	/** Add some bytes in the buffer.
+	@param inpData	pointer to bytes to add.
+	@param inDataLength	number of bytes to add.
+	@return true if all bytes have been pushed. false if one or more bytes are lost due to max size reached...
+	*/
+	bool PushBytes(byte* inpData, int inDataLength);
+
+	/** Get the next byte from the buffer.
+	@return first available byte, or 0.
+	*/
+	byte GetByte();
+
+	/** Get the next two bytes from the buffer, to form an integer.
+	@return integer created from two available bytes, or 0.
+	*/
+	int16_t GetInt16();
+
+	/** Get the next four bytes from the buffer, to form an integer.
+	@return integer created from two available bytes, or 0.
+	*/
+	int32_t GetInt32();
+
+	/** Get some bytes.
+	@param inpData	buffer to fill.
+	@param inDataLength	number of bytes to get.
+	@return true if all bytes have been get. false if there were not enough bytes in th buffer.
+	*/
+	bool GetBytes(byte* inpData, int inDataLength);
+
+	/** Get the next two bytes from the given buffer starting from the given position, to form an integer.
+	@param pBuffer	buffer to scan.
+	@param inPos	Position of the first useful byte.
+	@return integer created from two available bytes, or 0.
+	*/
+	static int16_t GetInt16(byte* pBuffer, int inPos);
+
+	/** Get the next four bytes from the given buffer starting from the given position, to form a 32 bits integer.
+	@param pBuffer	buffer to scan.
+	@param inPos	Position of the first useful byte.
+	@return integer created from four available bytes, or 0.
+	*/
+	static int32_t GetInt32(byte* pBuffer, int inPos);
+
+	/** Get some bytes from the given buffer starting from the given position.
+	@param pBuffer	buffer to scan.
+	@param inPos	Position of the first useful byte.
+	@param inpData	buffer to fill.
+	@param inDataLength	number of bytes to get.
+	@return true if all bytes have been get. false if there were not enough bytes in th buffer.
+	*/
+	static void GetBytes(byte *pBuffer, int inPos, byte* inpData, int inDataLength);
+
+	/** Count the number of bytes in the buffer.
+	@return number of bytes in the buffer.
+	*/
+	int GetCount();
+
+	/** Get the maximum size used by the buffer since the beggining of its usage.
+	@return maximum number of bytes in the buffer.
+	*/
+	int GetPeakCount() { return this->peakCount; }
+
+	/** Check if the buffer is empty or not.
+	*/
+	bool isEmpty() const
+	{
+		//if head and tail are equal, we are empty
+		return (!this->full && (this->head == this->tail));
+	}
+
+	/** Check if the buffer is full or not.
+	*/
+	bool isFull() const
+	{
+		//If tail is ahead the head by 1, we are full
+		return this->full;
+	}
+
+	/** Check if the begin has been called. If not, the buffer is not allocated and any usage will crash the system !
+	*/
+	bool CheckIfBeginDone();
+
+#ifdef DCCPP_DEBUG_MODE
+#ifdef VISUALSTUDIO
+	/** Unit test function
+	*/
+	static void Test();
+#endif
+
+	/** Print the list of messages in the stack.
+	@remark Only available if DCCPP_DEBUG_MODE is defined.
+	*/
+	void printCircularBuffer();
+#endif
+};
+
+#ifdef ARDUINO_ARCH_ESP32
+#define START_SEMAPHORE()	\
+	{ \
+		byte semaphoreTaken = this->xSemaphore == NULL?1:0; \
+		if (this->xSemaphore != NULL) \
+			if (xSemaphoreTake(this->xSemaphore, (TickType_t)100) == pdTRUE) \
+				semaphoreTaken = 1; \
+		if (semaphoreTaken == 1)
+
+#define END_SEMAPHORE()	\
+		xSemaphoreGive(this->xSemaphore); \
+	}
+
+#define ABORT_SEMAPHORE()	\
+		xSemaphoreGive(this->xSemaphore);
+#else
+#define START_SEMAPHORE()
+#define END_SEMAPHORE()
+#define ABORT_SEMAPHORE()
+#endif
+
+#endif

StringFormatter.cpp

@@ -24,6 +24,9 @@ bool Diag::ACK=false;
 bool Diag::CMD=false;
 bool Diag::WIFI=false;
 bool Diag::WITHROTTLE=false;
+bool Diag::Z21THROTTLE=true;
+bool Diag::Z21THROTTLEVERBOSE=true;
+bool Diag::Z21THROTTLEDATA=true;
 bool Diag::ETHERNET=false;
 bool Diag::LCN=false;
 

StringFormatter.h

@@ -28,6 +28,9 @@ class Diag {
   static bool CMD;
   static bool WIFI;
   static bool WITHROTTLE;
+  static bool Z21THROTTLE;
+  static bool Z21THROTTLEVERBOSE;
+  static bool Z21THROTTLEDATA;
   static bool ETHERNET;
   static bool LCN;
   

WifiESP32.cpp

@@ -27,6 +27,7 @@
 #include "RingStream.h"
 #include "CommandDistributor.h"
 #include "WiThrottle.h"
+#include "Z21Throttle.h"
 /*
 #include "soc/rtc_wdt.h"
 #include "esp_task_wdt.h"
@@ -113,6 +114,7 @@ bool WifiESP::setup(const char *SSid,
   bool havePassword = true;
   bool haveSSID = true;
   bool wifiUp = false;
+  IPAddress localIP;
   uint8_t tries = 40;
 
   //#ifdef SERIAL_BT_COMMANDS
@@ -152,7 +154,7 @@ bool WifiESP::setup(const char *SSid,
       delay(500);
     }
     if (WiFi.status() == WL_CONNECTED) {
-      DIAG(F("Wifi STA IP %s"),WiFi.localIP().toString().c_str());
+      DIAG(F("Wifi STA IP %s"),(localIP=WiFi.localIP()).toString().c_str());
       wifiUp = true;
     } else {
       DIAG(F("Could not connect to Wifi SSID %s"),SSid);
@@ -166,7 +168,7 @@ bool WifiESP::setup(const char *SSid,
 	delay(500);
       }
       if (WiFi.status() == WL_CONNECTED) {
-	DIAG(F("Wifi STA IP 2nd try %s"),WiFi.localIP().toString().c_str());
+	DIAG(F("Wifi STA IP 2nd try %s"),(localIP=WiFi.localIP()).toString().c_str());
 	wifiUp = true;
       } else {
 	DIAG(F("Wifi STA mode FAIL. Will revert to AP mode"));
@@ -195,7 +197,7 @@ bool WifiESP::setup(const char *SSid,
 		    havePassword ? password : strPass.c_str(),
 		    channel, false, 8)) {
       DIAG(F("Wifi AP SSID %s PASS %s"),strSSID.c_str(),havePassword ? password : strPass.c_str());
-      DIAG(F("Wifi AP IP %s"),WiFi.softAPIP().toString().c_str());
+      DIAG(F("Wifi AP IP %s"),(localIP=WiFi.softAPIP()).toString().c_str());
       wifiUp = true;
       APmode = true;
     } else {
@@ -212,6 +214,7 @@ bool WifiESP::setup(const char *SSid,
   server = new WiFiServer(port); // start listening on tcp port
   server->begin();
   // server started here
+  Z21Throttle::setup(localIP, Z21_UDPPORT);
 
 #ifdef WIFI_TASK_ON_CORE0
   //start loop task
@@ -297,6 +300,7 @@ void WifiESP::loop() {
     } // all clients
 
     WiThrottle::loop(outboundRing);
+    Z21Throttle::loop();
 
     // something to write out?
     clientId=outboundRing->read();

Z21Throttle.cpp

@@ -0,0 +1,846 @@
+/*
+ *  © 2023 Thierry Paris / Locoduino
+ *  All rights reserved.
+ *  
+ *  This file is part of CommandStation-EX-Labox
+ *
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+#include <Arduino.h>
+#include "defines.h"
+#include <WiFi.h>
+#include "Z21Throttle.h"
+#include "DCC.h"
+#include "WifiESP32.h"
+#include "DCCWaveform.h"
+#include "StringFormatter.h"
+#include "Turnouts.h"
+#include "DIAG.h"
+#include "GITHUB_SHA.h"
+#include "version.h"
+#include "EXRAIL2.h"
+#include "CommandDistributor.h"
+#include "TrackManager.h"
+#include "DCCTimer.h"
+#ifdef USE_HMI
+	#include "hmi.h"
+#endif
+
+static std::vector<NetworkClientUDP> clientsUDP; // a list to hold all UDP clients
+
+Z21Throttle *Z21Throttle::firstThrottle=NULL;
+byte Z21Throttle::commBuffer[100];
+byte Z21Throttle::replyBuffer[20];
+
+Z21Throttle* Z21Throttle::readWriteThrottle = NULL;
+int Z21Throttle::cvAddress = -1;
+int Z21Throttle::cvValue = -1;
+
+void printClientsUDP();
+
+WiFiUDP NetworkClientUDP::client;
+
+#define LOOPLOCOS(THROTTLECHAR, CAB)  for (int loco=0;loco<MAX_MY_LOCO;loco++) \
+      if ((myLocos[loco].throttle==THROTTLECHAR || '*'==THROTTLECHAR) && (CAB<0 || myLocos[loco].cab==CAB))
+
+void Z21Throttle::setup(IPAddress ip, int port) {
+	uint8_t ret = NetworkClientUDP::client.begin(ip, port);
+    if(ret == 1) DIAG(F("UDP Connection started port %d. Z21 apps are available."), port);
+    else DIAG(F("UDP Connection failed. Z21 apps not available."));
+	NetworkClientUDP::client.flush();
+}
+
+int readUdpPacket() {
+	byte udp[UDPBYTE_SIZE];
+	memset(udp, 0, UDPBYTE_SIZE);
+	int len = NetworkClientUDP::client.read(udp, UDPBYTE_SIZE);
+	if (len > 0) {
+		for (int clientId = 0; clientId < clientsUDP.size(); clientId++) {
+		if (clientsUDP[clientId].inUse)
+			if (clientsUDP[clientId].remoteIP == NetworkClientUDP::client.remoteIP() && clientsUDP[clientId].remotePort == NetworkClientUDP::client.remotePort()) {
+				clientsUDP[clientId].pudpBuffer->PushBytes(udp, len);
+			return clientId;
+			}
+		}
+	}
+
+	return -1;
+}
+
+void Z21Throttle::loop() {
+	int clientId = 0;
+
+	// loop over all clients and remove inactive
+	for (clientId = 0; clientId < clientsUDP.size(); clientId++) {
+		// check if client is there and alive
+		if (clientsUDP[clientId].inUse && !clientsUDP[clientId].connected) {
+			if (Diag::Z21THROTTLE) DIAG(F("Remove UDP client %d"), clientId);
+			clientsUDP[clientId].inUse = false;
+			printClientsUDP();
+		}
+	}
+
+	int len = NetworkClientUDP::client.parsePacket();
+	if (len > 0) {
+		int clientId = 0;
+		for (; clientId < clientsUDP.size(); clientId++) {
+			if (clientsUDP[clientId].inUse) {
+				if (clientsUDP[clientId].remoteIP == NetworkClientUDP::client.remoteIP() && clientsUDP[clientId].remotePort == NetworkClientUDP::client.remotePort()) {
+					//if (Diag::Z21THROTTLEVERBOSE) DIAG(F("UDP client %d : %s Already connected"), clientId, clientsUDP[clientId].remoteIP.toString().c_str());
+					break;
+				}
+			}
+		}
+
+		if (clientId >= clientsUDP.size()) {
+			NetworkClientUDP nc;
+			nc.remoteIP = NetworkClientUDP::client.remoteIP();
+			nc.remotePort = NetworkClientUDP::client.remotePort();
+			nc.connected = true;
+			nc.inUse = true;
+
+			clientsUDP.push_back(nc);
+			if (Diag::Z21THROTTLE) DIAG(F("New UDP client %d, %s"), clientId, nc.remoteIP.toString().c_str());
+			printClientsUDP();
+			#ifdef USE_HMI
+			if (hmi::CurrentInterface != NULL) hmi::CurrentInterface->NewClient(clientId, nc.remoteIP, 0);
+			#endif
+			// Fleischmann/Roco Android app starts with Power on !
+			TrackManager::setMainPower(POWERMODE::ON);
+		}
+
+		clientId = readUdpPacket();
+
+		if (clientId >= 0) {
+			if (clientsUDP[clientId].ok()) {        
+				Z21Throttle* pThrottle = getOrAddThrottle(clientId); 
+
+				if (pThrottle != NULL)
+					pThrottle->parse();
+			}
+		}
+	}
+}
+
+/** Print the list of assigned locomotives. */
+void Z21Throttle::printLocomotives(bool addTab) {
+	if (!Diag::Z21THROTTLE)
+		return;
+
+	DIAG(F("      Locomotives ------------------"));
+	for (int loco = 0; loco < MAX_MY_LOCO; loco++)
+		if (myLocos[loco].throttle != '\0')
+			DIAG(F("%s         %d : cab %d on throttle %c"), addTab ? "   ":"", loco, myLocos[loco].cab, myLocos[loco].throttle);
+}
+
+/** Print the list of assigned locomotives. */
+void printClientsUDP() {
+	if (!Diag::Z21THROTTLE) return;
+
+	DIAG(F("      UDP Clients ------------------"));
+	for (int clientId = 0; clientId < clientsUDP.size(); clientId++)
+		if (clientsUDP[clientId].ok())
+			DIAG(F("         %d %s: %s:%d"), clientId, clientsUDP[clientId].connected?"Connected":"Not connected", clientsUDP[clientId].remoteIP.toString().c_str(), clientsUDP[clientId].remotePort);
+		else
+			DIAG(F("         %d unused"), clientId);
+}
+
+/** Print the list of assigned locomotives. */
+void Z21Throttle::printThrottles(bool inPrintLocomotives) {
+	if (!Diag::Z21THROTTLE)	return;
+
+	DIAG(F("      Z21 Throttles ---------------"));
+	for (Z21Throttle* wt = firstThrottle; wt != NULL; wt = wt->nextThrottle) {
+		if (wt->clientid == -1)
+			DIAG(F("         unused"));
+		else {
+			DIAG(F("         %d : %d.%d.%d.%d:%d"), wt->clientid, 
+			clientsUDP[wt->clientid].remoteIP[0], 
+			clientsUDP[wt->clientid].remoteIP[1], 
+			clientsUDP[wt->clientid].remoteIP[2], 
+			clientsUDP[wt->clientid].remoteIP[3], 
+			clientsUDP[wt->clientid].remotePort);
+
+			if (inPrintLocomotives)
+				wt->printLocomotives(true);
+		}
+	}
+}
+
+Z21Throttle* Z21Throttle::getOrAddThrottle(int clientId) {
+	for (Z21Throttle* wt = firstThrottle; wt != NULL ; wt = wt->nextThrottle)  {
+		if (wt->clientid == clientId)
+			return wt; 
+	}
+
+	Z21Throttle *p = new Z21Throttle(clientId);
+	printThrottles(false);
+	return p;
+}
+
+void Z21Throttle::forget( byte clientId) {
+	for (Z21Throttle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle)  
+    	if (wt->clientid==clientId) {
+			delete wt;
+			break; 
+		}
+}
+
+bool Z21Throttle::isThrottleInUse(int cab) {
+	for (Z21Throttle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle)  
+		if (wt->areYouUsingThrottle(cab)) return true;
+
+	return false;
+}
+
+bool Z21Throttle::areYouUsingThrottle(int cab) {
+	LOOPLOCOS('*', cab) { // see if I have this cab in use
+		return true;
+	}
+
+	return false;
+}
+
+// One instance of Z21Throttle per connected client, so we know what the locos are 
+ 
+Z21Throttle::Z21Throttle(int inClientId) {
+	if (Diag::Z21THROTTLE) DIAG(F("New Z21Throttle for client UDP %d"), clientid); 
+	nextThrottle=firstThrottle;
+	firstThrottle= this;
+	clientid = inClientId;
+	initSent=false; // prevent sending heartbeats before connection completed
+	turnoutListHash = -1;  // make sure turnout list is sent once
+	exRailSent=false;
+	mostRecentCab=0;     
+	for (int loco=0;loco<MAX_MY_LOCO; loco++)
+		myLocos[loco].throttle='\0';
+}
+
+Z21Throttle::~Z21Throttle() {
+	if (Diag::Z21THROTTLE) DIAG(F("Deleting Z21Throttle client UDP %d"),this->clientid);
+	if (firstThrottle== this) {
+		firstThrottle=this->nextThrottle;
+		return;
+	}
+
+	for (Z21Throttle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) {
+		if (wt->nextThrottle==this) {
+			wt->nextThrottle=this->nextThrottle;
+			return;  
+		}
+	}
+}
+
+void Z21Throttle::write(byte* inpData, int inLengthData) {
+	size_t size = 0;
+
+//	if (this->dontReply)
+//		return;
+	
+	NetworkClientUDP::client.beginPacket(clientsUDP[this->clientid].remoteIP, clientsUDP[this->clientid].remotePort);
+	size = NetworkClientUDP::client.write(inpData, inLengthData);
+	NetworkClientUDP::client.endPacket();
+
+	if (Diag::Z21THROTTLEDATA) DIAG(F("Z21 Throttle %d : %s SENT 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x"), clientid,
+                          size == 0 ? "BINARY NOT" :"",
+                          (inLengthData > 0)?inpData[0]:0,
+                          (inLengthData > 1)?inpData[1]:0,
+                          (inLengthData > 2)?inpData[2]:0,
+                          (inLengthData > 3)?inpData[3]:0,
+                          (inLengthData > 4)?inpData[4]:0,
+                          (inLengthData > 5)?inpData[5]:0,
+                          (inLengthData > 6)?inpData[6]:0,
+                          (inLengthData > 7)?inpData[7]:0,
+                          (inLengthData > 8)?inpData[8]:0,
+                          (inLengthData > 9)?inpData[9]:0);
+}
+
+// sizes : [       2        ][       2        ][inLengthData]
+// bytes : [length1, length2][Header1, Header2][Data........]
+bool Z21Throttle::notify(unsigned int inHeader, byte* inpData, unsigned int inLengthData, bool inXorInData) {
+	int realLength = (inLengthData + 4 + (inXorInData == false ? 1 : 0));
+
+	Z21Throttle::commBuffer[0] = realLength % 256;
+	Z21Throttle::commBuffer[1] = realLength / 256;
+	Z21Throttle::commBuffer[2] = inHeader % 256;
+	Z21Throttle::commBuffer[3] = inHeader / 256;
+	memcpy(Z21Throttle::commBuffer + 4, inpData, inLengthData);
+
+	if (!inXorInData) {    // if xor byte not included in data, compute and write it !
+		byte xxor = 0;
+
+		for (unsigned int i = 0; i < inLengthData; i++)
+			xxor ^= inpData[i];
+
+		Z21Throttle::commBuffer[inLengthData+4] = xxor;
+	}
+
+	write(Z21Throttle::commBuffer, realLength);
+
+	return true;
+}
+
+// sizes : [       2        ][       2        ][   1   ][inLengthData]
+// bytes : [length1, length2][Header1, Header2][XHeader][Data........]
+bool Z21Throttle::notify(unsigned int inHeader, unsigned int inXHeader, byte* inpData, unsigned int inLengthData, bool inXorInData) {
+	int realLength = (inLengthData + 5 + (inXorInData == false ? 1 : 0));
+
+	Z21Throttle::commBuffer[0] = realLength % 256;
+	Z21Throttle::commBuffer[1] = realLength / 256;
+	Z21Throttle::commBuffer[2] = inHeader % 256;
+	Z21Throttle::commBuffer[3] = inHeader / 256;
+	Z21Throttle::commBuffer[4] = inXHeader;
+	memcpy(Z21Throttle::commBuffer + 5, inpData, inLengthData);
+
+	if (!inXorInData) {    // if xor byte not included in data, compute and write it !
+		byte xxor = inXHeader;
+
+		for (unsigned int i = 0; i < inLengthData; i++)
+			xxor ^= inpData[i];
+
+		Z21Throttle::commBuffer[inLengthData + 5] = xxor;
+	}
+
+	write(Z21Throttle::commBuffer, realLength);
+
+	return true;
+}
+
+// sizes : [       2        ][       2        ][   1   ][ 1 ][inLengthData]
+// bytes : [length1, length2][Header1, Header2][XHeader][DB0][Data........]
+bool Z21Throttle::notify(unsigned int inHeader, unsigned int inXHeader, byte inDB0, byte* inpData, unsigned int inLengthData, bool inXorInData) {
+	int realLength = (inLengthData + 6 + (inXorInData == false ? 1 : 0));
+
+	Z21Throttle::commBuffer[0] = realLength % 256;
+	Z21Throttle::commBuffer[1] = realLength / 256;
+	Z21Throttle::commBuffer[2] = inHeader % 256;
+	Z21Throttle::commBuffer[3] = inHeader / 256;
+	Z21Throttle::commBuffer[4] = inXHeader;
+	Z21Throttle::commBuffer[5] = inDB0;
+	memcpy(Z21Throttle::commBuffer + 6, inpData, inLengthData);
+
+	if (!inXorInData) {   // if xor byte not included in data, compute and write it !
+		byte xxor = inXHeader^inDB0;
+
+		for (unsigned int i = 0; i < inLengthData; i++)
+			xxor ^= inpData[i];
+
+		Z21Throttle::commBuffer[inLengthData + 6] = xxor;
+	}
+
+	write(Z21Throttle::commBuffer, realLength);
+
+	return true;
+}
+
+void Z21Throttle::notifyStatus() {
+	Z21Throttle::replyBuffer[0] = 0;	// main current 1
+	Z21Throttle::replyBuffer[1] = 0; // main current 2
+	Z21Throttle::replyBuffer[2] = 0; // prog current 1
+	Z21Throttle::replyBuffer[3] = 0; // prog current 2
+	Z21Throttle::replyBuffer[4] = 0; // filtered main current 1
+	Z21Throttle::replyBuffer[5] = 0; // filtered main current 2
+	Z21Throttle::replyBuffer[6] = 0; // Temperature 1
+	Z21Throttle::replyBuffer[7] = 0; // Temperature 2
+	Z21Throttle::replyBuffer[8] = 5; // Supply voltage 1
+	Z21Throttle::replyBuffer[9] = 0; // supply voltage 2
+	Z21Throttle::replyBuffer[10] = 16; // VCC voltage 1 
+	Z21Throttle::replyBuffer[11] = 0; // VCC voltage 2
+	Z21Throttle::replyBuffer[12] = 0b00000000;	// CentralState 
+	Z21Throttle::replyBuffer[13] = 0b00000000; // CentralStateEx
+	Z21Throttle::replyBuffer[14] = 0;
+	Z21Throttle::replyBuffer[15] = 0;
+	notify(HEADER_LAN_SYSTEMSTATE, Z21Throttle::replyBuffer, 16, true);
+}
+
+int Z21Throttle::getOrAddLoco(int cab) {
+	int loco = 0;
+	for (; loco < MAX_MY_LOCO; loco++) {
+		if (myLocos[loco].throttle != '\0' && myLocos[loco].cab == cab) 
+			return loco;
+	}
+
+	if (loco >= MAX_MY_LOCO) {
+		//use first empty "slot" on this client's list, will be added to DCC registration list
+		for (int locoToAdd = 0; locoToAdd < MAX_MY_LOCO; locoToAdd++) {
+			if (myLocos[locoToAdd].throttle == '\0') { 
+				myLocos[locoToAdd].throttle = '0' + this->clientid;
+				myLocos[locoToAdd].cab = cab; 
+				myLocos[locoToAdd].functionMap = DCC::getFunctionMap(cab); 
+				myLocos[locoToAdd].broadcastPending = true; // means speed/dir will be sent later
+				mostRecentCab = cab;
+				myLocos[locoToAdd].functionToggles = 1<<2; // F2 (HORN)  is a non-toggle
+				return locoToAdd;
+			}
+		}
+	}
+
+	return -1;  // no loco found, and no place to add one !
+}
+
+void Z21Throttle::notifyLocoInfo(byte inMSB, byte inLSB) {
+	int locoAddress = ((inMSB & 0x3F) << 8) + inLSB;
+	int loco = getOrAddLoco(locoAddress);
+
+	if (loco == -1)
+	    return; //'Too many locos !'
+
+	Z21Throttle::replyBuffer[0] = inMSB;	// loco address msb
+	Z21Throttle::replyBuffer[1] = inLSB; // loco address lsb
+	Z21Throttle::replyBuffer[2] = B00000100; // 0000CKKK	 C = already controlled    KKK = speed steps 000:14, 010:28, 100:128
+	Z21Throttle::replyBuffer[3] = DCC::getThrottleSpeed(locoAddress); // RVVVVVVV  R = forward    VVVVVVV = speed
+	if (DCC::getThrottleDirection(locoAddress)) bitSet(Z21Throttle::replyBuffer[3], 7);
+
+	Z21Throttle::replyBuffer[4] = B00000000; // 0DSLFGHJ  D = double traction    S = Smartsearch   L = F0   F = F4   G = F3   H = F2   J = F1
+	if (DCC::getFn(locoAddress, 0)) bitSet(Z21Throttle::replyBuffer[4], 4);
+	if (DCC::getFn(locoAddress, 1)) bitSet(Z21Throttle::replyBuffer[4], 0);
+	if (DCC::getFn(locoAddress, 2)) bitSet(Z21Throttle::replyBuffer[4], 1);
+	if (DCC::getFn(locoAddress, 3)) bitSet(Z21Throttle::replyBuffer[4], 2);
+	if (DCC::getFn(locoAddress, 4)) bitSet(Z21Throttle::replyBuffer[4], 3);
+
+	Z21Throttle::replyBuffer[5] = B00000000;	// function F5 to F12    F5 is bit0
+	if (DCC::getFn(locoAddress, 5)) bitSet(Z21Throttle::replyBuffer[5], 0);
+	if (DCC::getFn(locoAddress, 6)) bitSet(Z21Throttle::replyBuffer[5], 1);
+	if (DCC::getFn(locoAddress, 7)) bitSet(Z21Throttle::replyBuffer[5], 2);
+	if (DCC::getFn(locoAddress, 8)) bitSet(Z21Throttle::replyBuffer[5], 3);
+	if (DCC::getFn(locoAddress, 9)) bitSet(Z21Throttle::replyBuffer[5], 4);
+	if (DCC::getFn(locoAddress, 10)) bitSet(Z21Throttle::replyBuffer[5],5);
+	if (DCC::getFn(locoAddress, 11)) bitSet(Z21Throttle::replyBuffer[5],6);
+	if (DCC::getFn(locoAddress, 12)) bitSet(Z21Throttle::replyBuffer[5],7);
+
+	Z21Throttle::replyBuffer[6] = B00000000;	// function F13 to F20   F13 is bit0
+	if (DCC::getFn(locoAddress, 13)) bitSet(Z21Throttle::replyBuffer[6], 0);
+	if (DCC::getFn(locoAddress, 14)) bitSet(Z21Throttle::replyBuffer[6], 1);
+	if (DCC::getFn(locoAddress, 15)) bitSet(Z21Throttle::replyBuffer[6], 2);
+	if (DCC::getFn(locoAddress, 16)) bitSet(Z21Throttle::replyBuffer[6], 3);
+	if (DCC::getFn(locoAddress, 17)) bitSet(Z21Throttle::replyBuffer[6], 4);
+	if (DCC::getFn(locoAddress, 18)) bitSet(Z21Throttle::replyBuffer[6], 5);
+	if (DCC::getFn(locoAddress, 19)) bitSet(Z21Throttle::replyBuffer[6], 6);
+	if (DCC::getFn(locoAddress, 20)) bitSet(Z21Throttle::replyBuffer[6], 7);
+
+	Z21Throttle::replyBuffer[7] = B00000000;	// function F21 to F28   F21 is bit0
+	if (DCC::getFn(locoAddress, 21)) bitSet(Z21Throttle::replyBuffer[7], 0);
+	if (DCC::getFn(locoAddress, 22)) bitSet(Z21Throttle::replyBuffer[7], 1);
+	if (DCC::getFn(locoAddress, 23)) bitSet(Z21Throttle::replyBuffer[7], 2);
+	if (DCC::getFn(locoAddress, 24)) bitSet(Z21Throttle::replyBuffer[7], 3);
+	if (DCC::getFn(locoAddress, 25)) bitSet(Z21Throttle::replyBuffer[7], 4);
+	if (DCC::getFn(locoAddress, 26)) bitSet(Z21Throttle::replyBuffer[7], 5);
+	if (DCC::getFn(locoAddress, 27)) bitSet(Z21Throttle::replyBuffer[7], 6);
+	if (DCC::getFn(locoAddress, 28)) bitSet(Z21Throttle::replyBuffer[7], 7);
+  
+	notify(HEADER_LAN_XPRESS_NET, LAN_X_HEADER_LOCO_INFO, Z21Throttle::replyBuffer, 8, false);
+}
+
+void Z21Throttle::notifyTurnoutInfo(byte inMSB, byte inLSB) {
+	Z21Throttle::replyBuffer[0] = inMSB;	// turnout address msb
+	Z21Throttle::replyBuffer[1] = inLSB; // turnout address lsb
+	Z21Throttle::replyBuffer[2] = B00000000; // 000000ZZ	 ZZ : 00 not switched   01 pos1  10 pos2  11 invalid
+	notify(HEADER_LAN_XPRESS_NET, LAN_X_HEADER_TURNOUT_INFO, Z21Throttle::replyBuffer, 3, false);
+}
+
+void Z21Throttle::notifyLocoMode(byte inMSB, byte inLSB) {
+	Z21Throttle::replyBuffer[0] = inMSB;	// loco address msb
+	Z21Throttle::replyBuffer[1] = inLSB; // loco address lsb
+	Z21Throttle::replyBuffer[2] = B00000000; // 00000000	DCC   00000001 MM
+	notify(HEADER_LAN_GET_LOCOMODE, Z21Throttle::replyBuffer, 3, true);
+}
+
+void Z21Throttle::notifyFirmwareVersion() {
+	Z21Throttle::replyBuffer[0] = 0x01;	// Version major in BCD
+	Z21Throttle::replyBuffer[1] = 0x23;	// Version minor in BCD
+	notify(HEADER_LAN_XPRESS_NET, LAN_X_HEADER_FIRMWARE_VERSION, 0x0A, Z21Throttle::replyBuffer, 2, false);
+}
+
+void Z21Throttle::notifyHWInfo() {
+	Z21Throttle::replyBuffer[0] = 0x00;	// Hardware type in BCD on int32
+	Z21Throttle::replyBuffer[1] = 0x02;	// Hardware type in BCD on int32
+	Z21Throttle::replyBuffer[2] = 0x00;	// Hardware type in BCD on int32
+	Z21Throttle::replyBuffer[3] = 0x00;	// Hardware type in BCD on int32
+	Z21Throttle::replyBuffer[4] = 0x23;	// Firmware version in BCD on int32
+	Z21Throttle::replyBuffer[5] = 0x01;	// Firmware version in BCD on int32
+	Z21Throttle::replyBuffer[6] = 0x00;	// Firmware version in BCD on int32
+	Z21Throttle::replyBuffer[7] = 0x00;	// Firmware version in BCD on int32
+	notify(HEADER_LAN_GET_HWINFO, Z21Throttle::replyBuffer, 8, true);
+}
+
+void Z21Throttle::notifyCvNACK(int inCvAddress) {
+	Z21Throttle::replyBuffer[0] = highByte(inCvAddress); // cv address msb
+	Z21Throttle::replyBuffer[1] = lowByte(inCvAddress); // cv address lsb
+	notify(HEADER_LAN_XPRESS_NET, LAN_X_HEADER_CV_NACK, LAN_X_DB0_CV_NACK, Z21Throttle::replyBuffer, 0, false);
+}
+
+void Z21Throttle::notifyCvRead(int inCvAddress, int inValue) {
+	Z21Throttle::replyBuffer[0] = highByte(inCvAddress); // cv address msb
+	Z21Throttle::replyBuffer[1] = lowByte(inCvAddress); // cv address lsb
+	Z21Throttle::replyBuffer[2] = inValue; // cv value
+	notify(HEADER_LAN_XPRESS_NET, LAN_X_HEADER_CV_RESULT, 0x14, Z21Throttle::replyBuffer, 3, false);
+}
+
+void Z21Throttle::setSpeed(byte inNbSteps, byte inDB1, byte inDB2, byte inDB3) {
+	bool isForward = bitRead(inDB3, 7);
+	byte speed = inDB3;
+	bitClear(speed, 7);
+
+	if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : speed %d"), clientid, speed * (isForward ? 1:-1));
+
+	int locoAddress = ((inDB1 & 0x3F) << 8) + inDB2;
+
+	if (getOrAddLoco(locoAddress) == -1)
+    	return;
+
+	DCC::setThrottle(locoAddress, speed, isForward);
+
+	if ((this->broadcastFlags & BROADCAST_BASE) != 0)
+		notifyLocoInfo(inDB1, inDB2);
+}
+
+//
+// TODO Pass through a text message to avoid multi thread locks...
+//
+
+void Z21Throttle::setFunction(byte inDB1, byte inDB2, byte inDB3) {
+	// inDB3 :  TTNN NNNN		TT:00 off, TT:01 on; TT:10 toggle   NNNNNN function number
+
+	byte action = bitRead(inDB3, 6) + 2 * bitRead(inDB3, 7);
+	byte function = inDB3;
+	bitClear(function, 6);
+	bitClear(function, 7);
+	bool activeFlag = action == 0b01;
+
+	if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : function %d %s"), clientid, function, activeFlag?"ON":"OFF");
+
+	int locoAddress = ((inDB1 & 0x3F) << 8) + inDB2;
+	if (getOrAddLoco(locoAddress) == -1)
+    	return;
+
+	if (action == 0b10)	{	// toggle
+		bool isActivated = DCC::getFn(locoAddress, function);
+		activeFlag = !isActivated;
+	}
+
+	DCC::setFn(locoAddress, function, activeFlag);
+	if ((this->broadcastFlags & BROADCAST_BASE) != 0)
+		notifyLocoInfo(inDB1, inDB2);
+}
+
+//
+// TODO Pass through a text message to avoid multi thread locks...
+//
+
+void Z21CvValueCallback(int16_t inValue)
+{
+	Z21Throttle::cvValue = inValue;
+
+	if (inValue == -1)
+		Z21Throttle::readWriteThrottle->notifyCvNACK(Z21Throttle::cvAddress);
+	else
+		Z21Throttle::readWriteThrottle->notifyCvRead(Z21Throttle::cvAddress, inValue);
+
+	Z21Throttle::readWriteThrottle = NULL;
+}
+
+void Z21Throttle::cvReadProg(byte inDB1, byte inDB2) {
+	if (Z21Throttle::readWriteThrottle != NULL)
+		return;
+
+	int cvAddress = ((inDB1 & 0x3F) << 8) + inDB2 + 1;
+
+	if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : cvRead Prog %d"), clientid, cvAddress);
+
+	Z21Throttle::readWriteThrottle = this;
+	Z21Throttle::cvAddress = cvAddress - 1;
+
+	void (*ptr)(int16_t) = &Z21CvValueCallback;
+	DCC::readCV(cvAddress, ptr);
+}
+
+// Working as cvReadProg for the moment...
+void Z21Throttle::cvReadMain(byte inDB1, byte inDB2) {
+	if (Z21Throttle::readWriteThrottle != NULL)
+		return;
+
+	int cvAddress = ((inDB1 & 0x3F) << 8) + inDB2 + 1;
+
+	if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : cvRead Main cv %d"), clientid, cvAddress);
+
+	Z21Throttle::readWriteThrottle = this;
+	Z21Throttle::cvAddress = cvAddress - 1;
+
+	void (*ptr)(int16_t) = &Z21CvValueCallback;
+	DCC::readCV(cvAddress, ptr);
+}
+
+//
+// TODO Pass through a text message to avoid multi thread locks...
+//
+
+void Z21Throttle::cvWriteProg(byte inDB1, byte inDB2, byte inDB3) {
+	if (Z21Throttle::readWriteThrottle != NULL)
+		return;
+
+	int cvAddress = ((inDB1 & 0x3F) << 8) + inDB2 + 1;
+
+	if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : cvWrite Prog cv %d value %d"), clientid, cvAddress, inDB3);
+
+	Z21Throttle::readWriteThrottle = this;
+	Z21Throttle::cvAddress = cvAddress - 1;
+
+	void (*ptr)(int16_t) = &Z21CvValueCallback;
+	DCC::writeCVByte(cvAddress, inDB3, ptr);
+}
+
+// Working as cvReadProg for the moment...
+void Z21Throttle::cvReadPom(byte inDB1, byte inDB2, byte inDB3, byte inDB4) {
+	if (Z21Throttle::readWriteThrottle != NULL)
+		return;
+
+	int locoAddress = ((inDB1 & 0x3F) << 8) + inDB2;
+	int cvAddress = ((inDB3 & B00000011) << 8) + inDB4 + 1;
+
+	if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : cvRead Pom Loco %d cv %d"), clientid, locoAddress, cvAddress);
+
+	Z21Throttle::readWriteThrottle = this;
+	Z21Throttle::cvAddress = cvAddress - 1;
+
+	void (*ptr)(int16_t) = &Z21CvValueCallback;
+	DCC::readCV(cvAddress, ptr);
+}
+
+bool Z21Throttle::parse() {
+	bool done = false;
+	byte DB[100];
+	CircularBuffer* pBuffer = clientsUDP[this->clientid].pudpBuffer;
+
+	if (pBuffer == NULL)
+		return false;
+	if (pBuffer->isEmpty())
+		return false;
+
+	int lengthData = pBuffer->GetInt16() - 4;	// length of the data = total length - length of length (!) - length of header
+	int header = pBuffer->GetInt16();
+	byte Xheader = 0;
+	byte DB0 = 0;
+	int nbLocos = CountLocos();
+
+	if (lengthData > 0)	{
+		pBuffer->GetBytes(DB, lengthData);
+		if (Diag::Z21THROTTLEDATA) DIAG(F("%d <- len:%d  header:0x%02x  : 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x"),
+					this->clientid, lengthData, header,
+					(lengthData > 0)?DB[0]:0,
+					(lengthData > 1)?DB[1]:0,
+					(lengthData > 2)?DB[2]:0,
+					(lengthData > 3)?DB[3]:0,
+					(lengthData > 4)?DB[4]:0,
+					(lengthData > 5)?DB[5]:0,
+					(lengthData > 6)?DB[6]:0,
+					(lengthData > 7)?DB[7]:0,
+					(lengthData > 8)?DB[8]:0,
+					(lengthData > 9)?DB[9]:0);
+	}
+
+	switch (header)	{
+		case HEADER_LAN_XPRESS_NET:
+			Xheader = DB[0];
+			switch (Xheader) {
+				case LAN_X_HEADER_GENERAL:
+					DB0 = DB[1];
+					switch (DB0) {
+					case LAN_X_DB0_GET_VERSION:
+						if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d GET_VERSION"), this->clientid);
+						break;
+					case LAN_X_DB0_GET_STATUS:
+						if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d GET_STATUS  "), this->clientid);
+						notifyStatus();
+						done = true;
+						break;
+					case LAN_X_DB0_SET_TRACK_POWER_OFF:
+						if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d POWER_OFF"), this->clientid);
+						//
+						// TODO Pass through a text message to avoid multi thread locks...
+						//
+						TrackManager::setMainPower(POWERMODE::OFF);
+						done = true;
+						break;
+					case LAN_X_DB0_SET_TRACK_POWER_ON:
+						if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d POWER_ON"), this->clientid);
+						//
+						// TODO Pass through a text message to avoid multi thread locks...
+						//
+						TrackManager::setMainPower(POWERMODE::ON);
+						done = true;
+						break;
+					}
+					break;
+				case LAN_X_HEADER_SET_STOP:
+					if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d EMERGENCY_STOP"), this->clientid);
+					//
+					// TODO Pass through a text message to avoid multi thread locks...
+					//
+					//Emergency Stop  (speed code 1)
+					// setThrottle will cause a broadcast so notification will be sent
+					LOOPLOCOS('*', 0) { DCC::setThrottle(myLocos[loco].cab, 1, DCC::getThrottleDirection(myLocos[loco].cab)); }
+					done = true;
+					break;
+				case LAN_X_HEADER_SET_LOCO:
+					DB0 = DB[1];
+					switch (DB0) {
+						case LAN_X_DB0_LOCO_DCC14:
+							if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d LOCO DCC 14 SPEED"), this->clientid);
+							setSpeed(14, DB[2], DB[3], DB[4]);
+							done = true;
+							break;
+						case LAN_X_DB0_LOCO_DCC28:
+							if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d LOCO DCC 28 SPEED"), this->clientid);
+							setSpeed(28, DB[2], DB[3], DB[4]);
+							done = true;
+							break;
+						case LAN_X_DB0_LOCO_DCC128:
+							if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d LOCO DCC 128 SPEED"), this->clientid);
+							setSpeed(128, DB[2], DB[3], DB[4]);
+							done = true;
+							break;
+						case LAN_X_DB0_SET_LOCO_FUNCTION:
+							if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d LOCO DCC FUNCTION"), this->clientid);
+							setFunction(DB[2], DB[3], DB[4]);
+							if (Diag::Z21THROTTLE) {
+								// Debug capacity to print data...
+								byte function = DB[4];
+								bitClear(function, 6);
+								bitClear(function, 7);
+								if (function == 12) { // why not ?
+									printClientsUDP();
+									printThrottles(true);
+								}
+							}
+							done = true;
+							break;
+					}
+					break;
+				case LAN_X_HEADER_GET_LOCO_INFO:
+					if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d LOCO INFO: "), this->clientid);
+					notifyLocoInfo(DB[2], DB[3]);
+					done = true;
+
+					break;
+				case LAN_X_HEADER_GET_TURNOUT_INFO:
+					if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d TURNOUT INFO  "), this->clientid);
+					notifyTurnoutInfo(DB[1], DB[2]);
+					done = true;
+
+					break;
+				case LAN_X_HEADER_GET_FIRMWARE_VERSION:
+					if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d FIRMWARE VERSION  "), this->clientid);
+					notifyFirmwareVersion();
+					done = true;
+					break;
+				case LAN_X_HEADER_CV_READ:
+					if (TrackManager::getProgDriver() != NULL) {
+						if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d CV READ PROG "), this->clientid);
+						// DB0 should be 0x11
+						cvReadProg(DB[2], DB[3]);
+					}
+					else {
+						//
+						// TODO Dont work today...
+						//
+
+						// If no prog track, read on the main track !
+						if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d CV READ MAIN "), this->clientid);
+						// DB0 should be 0x11
+						cvReadMain(DB[2], DB[3]);
+					}
+					done = true;
+					break;
+				case LAN_X_HEADER_CV_POM:
+					if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d CV READ POM"), this->clientid);
+					// DB0 should be 0x11
+					cvReadPom(DB[2], DB[3], DB[4], DB[5]);
+					done = true;
+					break;
+				case LAN_X_HEADER_CV_WRITE:
+					if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d CV WRITE "), this->clientid);
+					notifyFirmwareVersion();
+					done = true;
+					break;
+				case LAN_X_HEADER_SET_TURNOUT:
+				case 0x22:
+					break;
+			}
+			break;
+
+		case HEADER_LAN_SET_BROADCASTFLAGS:
+			this->broadcastFlags = CircularBuffer::GetInt32(DB, 0);
+			if (Diag::Z21THROTTLEDATA) DIAG(F("BROADCAST FLAGS %d : %s %s %s %s %s %s %s %s %s %s %s"), this->clientid,
+							(this->broadcastFlags & BROADCAST_BASE)	? "BASE " : "" ,
+							(this->broadcastFlags & BROADCAST_RBUS)	? "RBUS " : "" ,
+							(this->broadcastFlags & BROADCAST_RAILCOM)	? "RAILCOM " : "" ,
+							(this->broadcastFlags & BROADCAST_SYSTEM)	? "SYSTEM " : "" ,
+							(this->broadcastFlags & BROADCAST_BASE_LOCOINFO)	? "LOCOINFO " : "" ,
+							(this->broadcastFlags & BROADCAST_LOCONET)	? "LOCONET " : "" ,
+							(this->broadcastFlags & BROADCAST_LOCONET_LOCO)	? "LOCONET_LOCO " : "" ,
+							(this->broadcastFlags & BROADCAST_LOCONET_SWITCH)	? "LOCONET_SWITCH " : "" ,
+							(this->broadcastFlags & BROADCAST_LOCONET_DETECTOR)	? "LOCONET_DETECTOR " : "" ,
+							(this->broadcastFlags & BROADCAST_RAILCOM_AUTO)	? "RAILCOM_AUTO " : "" ,
+							(this->broadcastFlags & BROADCAST_CAN)	? "CAN" : "" );
+			done = true;
+			break;
+		case HEADER_LAN_GET_LOCOMODE:
+			if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d GET LOCOMODE"), this->clientid);
+			notifyLocoMode(DB[0], DB[1]);	// big endian here, but resend the same as received, so no problem.
+			done = true;
+			break;
+
+		case HEADER_LAN_SET_LOCOMODE:
+			if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d SET LOCOMODE"), this->clientid);
+			done = true;
+			break;
+		case HEADER_LAN_GET_HWINFO:
+			if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d GET HWINFO"), this->clientid);
+			notifyHWInfo();	// big endian here, but resend the same as received, so no problem.
+			done = true;
+			break;
+		case HEADER_LAN_LOGOFF:
+			if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d LOGOFF"), this->clientid);
+			this->clientid = -1;
+			done = true;
+			break;
+		case HEADER_LAN_SYSTEMSTATE_GETDATA:
+			if (Diag::Z21THROTTLEVERBOSE) DIAG(F("%d SYSTEMSTATE GETDATA"), this->clientid);
+			notifyStatus();	// big endian here, but resend the same as received, so no problem.
+			done = true;
+			break;
+		case HEADER_LAN_GET_SERIAL_NUMBER:
+		case HEADER_LAN_GET_BROADCASTFLAGS:
+		case HEADER_LAN_GET_TURNOUTMODE:
+		case HEADER_LAN_SET_TURNOUTMODE:
+		case HEADER_LAN_RMBUS_DATACHANGED:
+		case HEADER_LAN_RMBUS_GETDATA:
+		case HEADER_LAN_RMBUS_PROGRAMMODULE:
+		case HEADER_LAN_RAILCOM_DATACHANGED:
+		case HEADER_LAN_RAILCOM_GETDATA:
+		case HEADER_LAN_LOCONET_DISPATCH_ADDR:
+		case HEADER_LAN_LOCONET_DETECTOR:
+			break;
+	}
+
+	if (!done) {
+    	if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : not treated :  header:%x   Xheader:%x   DB0:%x"), this->clientid, header, Xheader, DB0);
+	}
+	else {
+		int newNbLocos = CountLocos();
+		if (nbLocos != newNbLocos)
+			printLocomotives();
+	}
+	return true;
+}

Z21Throttle.h

@@ -0,0 +1,223 @@
+/*
+ *  © 2023 Thierry Paris / Locoduino
+ *  All rights reserved.
+ *  
+ *  This is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  It is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with CommandStation.  If not, see <https://www.gnu.org/licenses/>.
+ */
+#ifndef Z21Throttle_h
+#define Z21Throttle_h
+
+#include "CircularBuffer.hpp"
+#include "WiFiClient.h"
+
+#define UDPBYTE_SIZE	64
+#define UDP_BUFFERSIZE	256
+
+struct MYLOCOZ21 {
+    char throttle; //indicates which throttle letter on client, '0' + clientid
+    int cab; //address of this loco
+    bool broadcastPending;
+    uint32_t functionMap;
+    uint32_t functionToggles;
+};
+
+class NetworkClientUDP {
+	public:
+	NetworkClientUDP() {
+		this->pudpBuffer = new CircularBuffer(UDP_BUFFERSIZE);
+		this->pudpBuffer->begin(true);
+	};
+	bool ok() {
+		return (inUse);
+	};
+
+	bool inUse = true;
+	bool connected = false;
+	CircularBuffer *pudpBuffer = NULL;
+	IPAddress remoteIP;
+	int remotePort;
+
+	static WiFiUDP client;
+};
+
+class Z21Throttle {
+	public:  
+		static void loop();
+		static Z21Throttle* getOrAddThrottle(int clientId); 
+		static void markForBroadcast(int cab);
+		static void forget(byte clientId);
+		static void findUniqThrottle(int id, char *u);
+		static void setup(IPAddress ip, int port);
+
+		void notifyCvNACK(int inCvAddress);
+		void notifyCvRead(int inCvAddress, int inValue);
+		
+		bool parse();
+
+		static Z21Throttle *readWriteThrottle;	// NULL if no throttle is reading or writing a CV...
+		static int cvAddress;
+		static int cvValue;
+
+	private: 
+		Z21Throttle(int clientId);
+		~Z21Throttle();
+
+		static const int MAX_MY_LOCO=10;      // maximum number of locos assigned to a single client
+		static const int HEARTBEAT_SECONDS=10; // heartbeat at 4secs to provide messaging transport
+		static const int ESTOP_SECONDS=20;     // eStop if no incoming messages for more than 8secs
+		static Z21Throttle* firstThrottle;
+		static byte commBuffer[100];
+		static byte replyBuffer[20];
+		static char LorS(int cab); 
+		static bool isThrottleInUse(int cab);
+		static void setSendTurnoutList();
+		bool areYouUsingThrottle(int cab);
+		Z21Throttle* nextThrottle;
+
+		int clientid;
+		char uniq[17] = "";
+
+		MYLOCOZ21 myLocos[MAX_MY_LOCO];   
+
+		int CountLocos() {  
+			int count = 0;
+			for (int loco=0;loco<MAX_MY_LOCO;loco++)
+				if (myLocos[loco].throttle != '\0')
+					count++;
+
+			return count;
+		}
+
+		bool initSent; // valid connection established
+		bool exRailSent; // valid connection established
+		uint16_t mostRecentCab;
+		int turnoutListHash;  // used to check for changes to turnout list
+		bool lastPowerState;  // last power state sent to this client
+		int32_t broadcastFlags;
+
+		int getOrAddLoco(int cab);
+		void printLocomotives(bool addTab = false);
+		static void printThrottles(bool printLocomotives);
+
+		// sizes : [       2        ][       2        ][inLengthData]
+		// bytes : [length1, length2][Header1, Header2][Data........]
+		bool notify(unsigned int inHeader, byte* inpData, unsigned int inLengthData, bool inXorInData);
+
+		// sizes : [       2        ][       2        ][   1   ][inLengthData]
+		// bytes : [length1, length2][Header1, Header2][XHeader][Data........]
+		bool notify(unsigned int inHeader, unsigned int inXHeader, byte* inpData, unsigned int inLengthData, bool inXorInData);
+
+		// sizes : [       2        ][       2        ][   1   ][ 1 ][inLengthData]
+		// bytes : [length1, length2][Header1, Header2][XHeader][DB0][Data........]
+		bool notify(unsigned int inHeader, unsigned int inXHeader, byte inDB0, byte* inpData, unsigned int inLengthData, bool inXorInData);
+
+		void notifyStatus();
+		void notifyLocoInfo(byte inMSB, byte inLSB);
+		void notifyTurnoutInfo(byte inMSB, byte inLSB);
+		void notifyLocoMode(byte inMSB, byte inLSB);
+		void notifyFirmwareVersion();
+		void notifyHWInfo();
+		void write(byte* inpData, int inLengthData);
+
+		void setSpeed(byte inNbSteps, byte inDB1, byte inDB2, byte inDB3);
+		void setFunction(byte inDB1, byte inDB2, byte inDB3);
+		void cvReadProg(byte inDB1, byte inDB2);
+		void cvWriteProg(byte inDB1, byte inDB2, byte inDB3);
+		void cvReadMain(byte inDB1, byte inDB2);
+		void cvReadPom(byte inDB1, byte inDB2, byte inDB3, byte inDB4);
+
+		// callback stuff to support prog track acquire
+		static Z21Throttle * stashInstance;
+		static byte         stashClient;
+		static char         stashThrottleChar;
+		static void         getLocoCallback(int16_t locoid);
+};
+
+#define Z21_UDPPORT		21105
+#define Z21_TIMEOUT		60000		// if no activity during 1 minute, disconnect the throttle...
+
+#define HEADER_LAN_GET_SERIAL_NUMBER 0x10
+#define HEADER_LAN_LOGOFF 0x30
+#define HEADER_LAN_XPRESS_NET 0x40
+#define HEADER_LAN_SET_BROADCASTFLAGS 0x50
+#define HEADER_LAN_GET_BROADCASTFLAGS 0x51
+#define HEADER_LAN_SYSTEMSTATE_GETDATA 0x85    //0x141 0x21 0x24 0x05
+#define HEADER_LAN_GET_HWINFO 0x1A
+#define HEADER_LAN_GET_LOCOMODE 0x60
+#define HEADER_LAN_SET_LOCOMODE 0x61
+#define HEADER_LAN_GET_TURNOUTMODE 0x70
+#define HEADER_LAN_SET_TURNOUTMODE 0x71
+#define HEADER_LAN_RMBUS_DATACHANGED 0x80
+#define HEADER_LAN_RMBUS_GETDATA 0x81
+#define HEADER_LAN_RMBUS_PROGRAMMODULE 0x82
+#define HEADER_LAN_RAILCOM_DATACHANGED 0x88
+#define HEADER_LAN_RAILCOM_GETDATA 0x89
+#define HEADER_LAN_LOCONET_DISPATCH_ADDR 0xA3
+#define HEADER_LAN_LOCONET_DETECTOR 0xA4
+
+#define LAN_GET_CONFIG 0x12
+
+#define LAN_X_HEADER_GENERAL 0x21
+#define LAN_X_HEADER_SET_STOP 0x80
+#define LAN_X_HEADER_GET_FIRMWARE_VERSION 0xF1  //0x141 0x21 0x21 0x00 
+#define LAN_X_HEADER_GET_LOCO_INFO 0xE3
+#define LAN_X_HEADER_SET_LOCO 0xE4
+#define LAN_X_HEADER_GET_TURNOUT_INFO 0x43
+#define LAN_X_HEADER_SET_TURNOUT 0x53
+#define LAN_X_HEADER_CV_READ 0x23
+#define LAN_X_HEADER_CV_WRITE 0x24
+#define LAN_X_HEADER_CV_POM 0xE6
+
+#define LAN_X_DB0_GET_VERSION 0x21
+#define LAN_X_DB0_GET_STATUS 0x24
+#define LAN_X_DB0_SET_TRACK_POWER_OFF 0x80
+#define LAN_X_DB0_SET_TRACK_POWER_ON 0x81
+#define LAN_X_DB0_LOCO_DCC14	0x10
+#define LAN_X_DB0_LOCO_DCC28	0x12
+#define LAN_X_DB0_LOCO_DCC128	0x13
+#define LAN_X_DB0_SET_LOCO_FUNCTION 0xF8
+#define LAN_X_DB0_CV_POM_WRITE 0x30
+#define LAN_X_DB0_CV_POM_ACCESSORY_WRITE 0x31
+
+#define LAN_X_OPTION_CV_POM_WRITE_BYTE 0xEC
+#define LAN_X_OPTION_CV_POM_WRITE_BIT 0xE8
+#define LAN_X_OPTION_CV_POM_READ_BYTE 0xE4
+
+// Replies to the controllers
+#define HEADER_LAN_SYSTEMSTATE	0x84
+
+#define LAN_X_HEADER_LOCO_INFO	0xEF
+#define LAN_X_HEADER_TURNOUT_INFO 0x43
+#define LAN_X_HEADER_FIRMWARE_VERSION 0xF3
+#define LAN_X_HEADER_CV_NACK 0x61
+#define LAN_X_HEADER_CV_RESULT 0x64
+
+#define LAN_X_DB0_CV_NACK_SC 0x12
+#define LAN_X_DB0_CV_NACK 0x13
+
+// Broadcast flags
+
+#define BROADCAST_BASE				0x00000001
+#define BROADCAST_RBUS				0x00000002
+#define BROADCAST_RAILCOM			0x00000004
+#define BROADCAST_SYSTEM			0x00000100
+#define BROADCAST_BASE_LOCOINFO		0x00010000
+#define BROADCAST_LOCONET			0x01000000
+#define BROADCAST_LOCONET_LOCO		0x02000000
+#define BROADCAST_LOCONET_SWITCH	0x04000000
+#define BROADCAST_LOCONET_DETECTOR	0x08000000
+#define BROADCAST_RAILCOM_AUTO		0x00040000
+#define BROADCAST_CAN				0x00080000
+
+#endif