From a3274caf49f116d59458a335c1f9005a84164ba0 Mon Sep 17 00:00:00 2001 From: Harald Barth Date: Tue, 6 Jun 2023 00:50:03 +0200 Subject: [PATCH] copy in --- CircularBuffer.cpp | 387 ++++++++++++++++++++ CircularBuffer.hpp | 185 ++++++++++ StringFormatter.cpp | 3 + StringFormatter.h | 3 + WifiESP32.cpp | 10 +- Z21Throttle.cpp | 846 ++++++++++++++++++++++++++++++++++++++++++++ Z21Throttle.h | 223 ++++++++++++ 7 files changed, 1654 insertions(+), 3 deletions(-) create mode 100644 CircularBuffer.cpp create mode 100644 CircularBuffer.hpp create mode 100644 Z21Throttle.cpp create mode 100644 Z21Throttle.h diff --git a/CircularBuffer.cpp b/CircularBuffer.cpp new file mode 100644 index 0000000..2f8016f --- /dev/null +++ b/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 . + */ + +#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 diff --git a/CircularBuffer.hpp b/CircularBuffer.hpp new file mode 100644 index 0000000..76e0e29 --- /dev/null +++ b/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 . + */ + +//------------------------------------------------------------------- +#ifndef __CircularBuffer_Hpp__ +#define __CircularBuffer_Hpp__ +//------------------------------------------------------------------- + +#include +#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 \ No newline at end of file diff --git a/StringFormatter.cpp b/StringFormatter.cpp index f7d9c50..470972e 100644 --- a/StringFormatter.cpp +++ b/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; diff --git a/StringFormatter.h b/StringFormatter.h index 6923c10..ab8118f 100644 --- a/StringFormatter.h +++ b/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; diff --git a/WifiESP32.cpp b/WifiESP32.cpp index 7754e11..cd10ffc 100644 --- a/WifiESP32.cpp +++ b/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(); diff --git a/Z21Throttle.cpp b/Z21Throttle.cpp new file mode 100644 index 0000000..4ccbec5 --- /dev/null +++ b/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 . + */ +#include +#include "defines.h" +#include +#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 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 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;lococlientid); + 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; +} diff --git a/Z21Throttle.h b/Z21Throttle.h new file mode 100644 index 0000000..3d061f6 --- /dev/null +++ b/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 . + */ +#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