From 669356df7d2ef4d9fe6396ecdd2151d60f1d1bb0 Mon Sep 17 00:00:00 2001 From: Asbelos Date: Sat, 27 Jun 2020 15:36:32 +0100 Subject: [PATCH] First WiThrottle prototype --- DCC.cpp | 12 +++ DCC.h | 2 + WiThrottle.cpp | 238 ++++++++++++++++++++++++++++++++++++++++++++++ WiThrottle.h | 38 ++++++++ WifiInterface.cpp | 17 +++- 5 files changed, 302 insertions(+), 5 deletions(-) create mode 100644 WiThrottle.cpp create mode 100644 WiThrottle.h diff --git a/DCC.cpp b/DCC.cpp index f776b04..e1c6d39 100644 --- a/DCC.cpp +++ b/DCC.cpp @@ -62,6 +62,18 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) { DCCWaveform::mainTrack.schedulePacket(b, nB, 3); // send packet 3 times } +uint8_t DCC::getThrottleSpeed(int cab) { + int reg=lookupSpeedTable(cab); + if (reg<0) return -1; + return speedTable[reg].speedCode & 0x7F; +} + +bool DCC::getThrottleDirection(int cab) { + int reg=lookupSpeedTable(cab); + if (reg<0) return false ; + return (speedTable[reg].speedCode & 0x80) !=0; +} + static void DCC::setFn( int cab, byte functionNumber, bool on) { if (cab<=0 || functionNumber<0 || functionNumber>28) return; int reg = lookupSpeedTable(cab); diff --git a/DCC.h b/DCC.h index 18680cf..a920073 100644 --- a/DCC.h +++ b/DCC.h @@ -36,6 +36,8 @@ class DCC { // Public DCC API functions static void setThrottle( uint16_t cab, uint8_t tSpeed, bool tDirection); + static uint8_t getThrottleSpeed(int cab); + static bool getThrottleDirection(int cab); static void writeCVByteMain(int cab, int cv, byte bValue); static void writeCVBitMain(int cab, int cv, byte bNum, bool bValue); static void setFunction( int cab, byte fByte, byte eByte); diff --git a/WiThrottle.cpp b/WiThrottle.cpp new file mode 100644 index 0000000..4f35ed7 --- /dev/null +++ b/WiThrottle.cpp @@ -0,0 +1,238 @@ +/* + * Truncated JMRI WiThrottle server implementation for DCC-EX command station + * Credit is due to Valerie Valley RR https://sites.google.com/site/valerievalleyrr/ + * for showing how it could be done, but this code is very different to the original + * implemenatatin as it is designed to run on the Arduino and not the ESP and is + * also calling directly into the DCCEX Api rather than simulating JMRI text commands. + * Refer JMRI WiFi Throttle Communications Protocol http://jmri.sourceforge.net/help/en/package/jmri/jmrit/withrottle/Protocol.shtml + * + * + * PROTOTYPE NOTES: + * There will be one WiThrottle instance created for each WiThrottle client detected by the WifiInterface. + * Some shortcuts have been taken and there are some things that are yet to be included: + * e.g. Full response to adding a loco. + * What to do about unknown turnouts. + * Broadcasting to other WiThrottles when things change. + * - Bear in mind that changes may have taken place due to + * other WiThrottles, OR JMRI commands received OR TPL automation. + * - I suggest that at the end of parse(), then anything that has changed and is of interest could + * be notified then. (e.g loco speeds, directions or functions, turnout states. + * + * WiThrottle.h sets the max locos per client at 10, this is ok to increase but requires just an extra 3 bytes per loco per client. +*/ +#include +#include "WiThrottle.h" +#include "DCC.h" +#include "DCCWaveform.h" +#include "StringFormatter.h" +#include "Turnouts.h" +#include "DIAG.h" + +#define LOOPLOCOS(THROTTLECHAR, CAB) for (int loco=0;loconextThrottle) + if (wt->clientid==wifiClient) return wt; + return new WiThrottle(stream, wifiClient); +} + + // One instance of WiTHrottle per connected client, so we know what the locos are + +WiThrottle::WiThrottle(Print & stream, int wificlientid) { + DIAG(F("\nCreating new WiThrottle for client %d\n"),wificlientid); + nextThrottle=firstThrottle; + firstThrottle= this; + clientid=wificlientid; + for (int loco=0;loconextTurnout){ + StringFormatter::send(stream,F("]\\[LT&d}|{%d}|{%d"), tt->data.id, tt->data.id, (bool)(tt->data.tStatus & STATUS_ACTIVE)); + } + StringFormatter::send(stream,F("\n*10")); +} + +WiThrottle::~WiThrottle() { + if (firstThrottle== this) { + firstThrottle=this->nextThrottle; + return; + } + for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) { + if (wt->nextThrottle==this) { + wt->nextThrottle=this->nextThrottle; + return; + } + } +} + +void WiThrottle::parse(Print & stream, char * cmd) { + heartBeat=millis(); + DIAG(F("\nWiThrottle parse (%d) %s"),clientid, cmd); + + switch (cmd[0]) { + case '*': // heartbeat control + if (cmd[1]=='+') heartBeatEnable=true; + else if (cmd[1]=='-') heartBeatEnable=false; + break; + case 'P': + if (cmd[1]=='P' && cmd[2]=='A' ) { //PPA power mode + DCCWaveform::mainTrack.setPowerMode(cmd[3]=='1'?POWERMODE::ON:POWERMODE::OFF); + StringFormatter::send(stream, F("PPA%c"),cmd[3]); + } + else if (cmd[1]='T' && cmd[2]=='A') { // PTA accessory toggle + // TODO... if we are given an address that is not a known Turnout... + // should we create one or just send the DCC message. + Turnout::activate(getInt(cmd+4),cmd[3]=='T'); + } + break; + case 'N': // Heartbeat (2) + StringFormatter::send(stream, F("*10")); // 10 second timeout + break; + case 'M': // multithrottle + multithrottle(stream, cmd); + break; + } +} +int WiThrottle::getInt(char * cmd) { + int i=0; + while (cmd[0]>='0' && cmd[0]<='9') { + i=i*10 + (cmd[0]-'0'); + cmd++; + } + return i; +} + +int WiThrottle::getLocoId(char * cmd) { + if (cmd[0]=='*') return -1; // match all locos + if (cmd[0]!='L' && cmd[0]!='S') return 0; // should not match any locos + return getInt(cmd+1); +} +void WiThrottle::multithrottle(Print & stream, char* cmd){ + char throttleChar=cmd[1]; + int locoid=getLocoId(cmd+3); // -1 for * + char * aval=cmd; + while(*aval !=';' && *aval !='\0') aval++; + if (*aval) aval++; + + switch(cmd[2]) { + case '+': // add loco + for (int loco=0;loco\n"), throttleChar, cmd[3] ,locoid); + // TODO... get known Fn states from DCC (need memoryStream improvements to handle data length) + // for(fKey=0; fKey<29; fKey++)StringFormatter::send(stream,F("M%cA%c<;>F0&s\n"),throttleChar,cmd[3],fkey); + StringFormatter::send(stream, F("M%c+%c%d<;>V0\n"), throttleChar, cmd[3], locoid); + StringFormatter::send(stream, F("M%c+%c%d<;>R1\n"), throttleChar, cmd[3], locoid); + StringFormatter::send(stream, F("M%c+%c%d<;>s1\n"), throttleChar, cmd[3], locoid); + break; + } + } + break; + case '-': // remove loco + LOOPLOCOS(throttleChar, locoid) { + myLocos[loco].throttle='\0'; + DCC::setThrottle(myLocos[loco].cab,0,0); + StringFormatter::send(stream, F("M%c-<;>\n"), throttleChar); + } + + break; + case 'A': + locoAction(stream,aval, throttleChar, locoid); + } +} + + + + +/*** TODO provide add feedback *** + +void locoAdd(String th, String actionKey, int i) { + LocoThrottle[Throttle] = actionKey; + client[i].println("M"+th+"+"+actionKey+"<;>"); + for(fKey=0; fKey<29; fKey++){ + LocoState[Throttle][fKey] =0; + client[i].println("M"+th+"A"+actionKey+"<;>F0"+String(fKey)); + } + client[i].println("M"+th+"+"+actionKey+"<;>V0"); + client[i].println("M"+th+"+"+actionKey+"<;>R1"); + client[i].println("M"+th+"+"+actionKey+"<;>s1"); +} +*********/ + +void WiThrottle::locoAction(Print & stream, char* aval, char throttleChar, int cab){ + // Note cab=-1 for all cabs in the consist called throttleChar. + + switch (aval[0]) { + case 'V': // Vspeed + { + byte locospeed=getInt(aval+1); + LOOPLOCOS(throttleChar, cab) { + DCC::setThrottle(myLocos[loco].cab,locospeed, DCC::getThrottleDirection(myLocos[loco].cab)); + } + } + break; + case 'F': //F onOff function + { + bool onOff=aval[1]=='1'; + int fKey = getInt(aval+2); + LOOPLOCOS(throttleChar, cab) { + DCC::setFn(myLocos[loco].cab, fKey,onOff); + } + } + break; + case 'q': + if (aval[1]=='V') { //qV + LOOPLOCOS(throttleChar, cab) { + StringFormatter::send(stream,F("M%cAL%d<;>V%d"), throttleChar, myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab)); + } + } + else if (aval[1]=='R') { // qR + LOOPLOCOS(throttleChar, cab) { + StringFormatter::send(stream,F("M%cAL%d<;>R%d"), throttleChar, myLocos[loco].cab, DCC::getThrottleDirection(myLocos[loco].cab)); + } + } + break; + case 'R': + { + bool forward=aval[1]!='0'; + LOOPLOCOS(throttleChar, cab) { + DCC::setThrottle(myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab), forward); + } + } + break; + case 'X': + //Emergency Stop (TODO check we have the correct speed code here) + LOOPLOCOS(throttleChar, cab) { + DCC::setThrottle(myLocos[loco].cab,1, DCC::getThrottleDirection(myLocos[loco].cab)); + } + break; + case 'I': // Idle + case 'Q': // Quit + LOOPLOCOS(throttleChar, cab) { + DCC::setThrottle(myLocos[loco].cab,0, DCC::getThrottleDirection(myLocos[loco].cab)); + } + break; + } +} + +void WiThrottle::loop() { + // for each WiThrottle, check the heartbeat + for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) + wt->checkHeartbeat(); +} + +void WiThrottle::checkHeartbeat() { + if(millis()-heartBeat > HEARTBEAT_TIMEOUT*1000) { + for (int loco=0;locoV0"),myLocos[loco].throttle,myLocos[loco].cab); + } + } + } +} diff --git a/WiThrottle.h b/WiThrottle.h new file mode 100644 index 0000000..64ecf54 --- /dev/null +++ b/WiThrottle.h @@ -0,0 +1,38 @@ +#ifndef WiThrottle_h +#define WiTHrottle_h + + +struct MYLOCO { + char throttle; + int cab; +}; + +class WiThrottle { + public: + static void loop(); + void parse(Print & stream, char * cmd); + static WiThrottle* getThrottle(Print & stream, int wifiClient); + + private: + WiThrottle(Print & stream, int wifiClientId); + ~WiThrottle(); + + static const int MAX_MY_LOCO=10; + static const int HEARTBEAT_TIMEOUT=10; + static WiThrottle* firstThrottle; + static int getInt(char * cmd); + static int getLocoId(char * cmd); + + WiThrottle* nextThrottle; + int clientid; + + MYLOCO myLocos[MAX_MY_LOCO]; + bool heartBeatEnable; + unsigned long heartBeat; + + void multithrottle(Print & stream, char* cmd); + void locoAction(Print & stream, char* aval, char throttleChar, int cab); + void accessory(Print & stream, char* cmd); + void checkHeartbeat(); +}; +#endif diff --git a/WifiInterface.cpp b/WifiInterface.cpp index c5195ba..a8b4641 100644 --- a/WifiInterface.cpp +++ b/WifiInterface.cpp @@ -2,7 +2,7 @@ #include "Config.h" #include "DIAG.h" #include "StringFormatter.h" - +#include "WiThrottle.h" const char PROGMEM READY_SEARCH[] ="\r\nready\r\n"; const char PROGMEM OK_SEARCH[] ="\r\nOK\r\n"; @@ -77,7 +77,9 @@ bool WifiInterface::checkForOK( const int timeout, const char * waitfor, bool ec void WifiInterface::loop() { if (!connected) return; - + + WiThrottle::loop(); // check heartbeats + // read anything into a buffer, collecting info on the way while (loopstate!=99 && Serial1.available()) { int ch=Serial1.read(); @@ -116,7 +118,6 @@ void WifiInterface::loop() { } // switch } // while if (loopstate!=99) return; - // TODO remove > in data streamer.write('\0'); DIAG(F("\nWifiRead:%d:%s\n"),connectionId,buffer); @@ -125,9 +126,15 @@ void WifiInterface::loop() { // We know that parser will read the entire buffer before starting to write to it. // Otherwise we would have to copy the buffer elsewhere and RAM is in short supply. - // TODO ... tell parser that callbacks are diallowed because we dont want to handle the async - parser.parse(streamer,buffer); + // TODO ... tell JMRI parser that callbacks are diallowed because we dont want to handle the async + + if (buffer[0]=='<') parser.parse(streamer,buffer); + else WiThrottle::getThrottle(streamer, connectionId)->parse(streamer, buffer); + + if (streamer.available()) { // there is a reply to send + DIAG(F("WiFiInterface Responding (%d) %s\n"),connectionId,buffer); + StringFormatter::send(Serial1,F("AT+CIPSEND=%d,%d\r\n"),connectionId,streamer.available()); streamer.write('\0'); if (checkForOK(1000,PROMPT_SEARCH,true)) Serial1.print((char *) buffer);