mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2024-11-23 16:16:13 +01:00
copy in
This commit is contained in:
Harald Barth
parent
8a69403dda
commit
a3274caf49
387
CircularBuffer.cpp
Normal file
387
CircularBuffer.cpp
Normal file
|
|
@ -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
|
||||
185
CircularBuffer.hpp
Normal file
185
CircularBuffer.hpp
Normal file
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
846
Z21Throttle.cpp
Normal file
846
Z21Throttle.cpp
Normal file
|
|
@ -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;
|
||||
}
|
||||
223
Z21Throttle.h
Normal file
223
Z21Throttle.h
Normal file
|
|
@ -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
|
||||
Loading…
Reference in New Issue
Block a user