mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2025-07-29 10:23:45 +02:00
Compare commits
90 Commits
v5.2.54-De
...
devel-hear
Author | SHA1 | Date | |
---|---|---|---|
|
88caef89f1 | ||
|
f7496b7853 | ||
|
1d18d5dea5 | ||
|
21c01ab69a | ||
|
fa00e9e11b | ||
|
f5014f5595 | ||
|
33c8ed19a9 | ||
|
0e99ad143b | ||
|
01533e2cd2 | ||
|
07ab7286ba | ||
|
dc481a2f0c | ||
|
692f97e480 | ||
|
7fb7751f19 | ||
|
546ddd8139 | ||
|
4aa353edbc | ||
|
c1d6ee2804 | ||
|
14360b4198 | ||
|
dd898d3c16 | ||
|
277431e84c | ||
|
fe2f705fa9 | ||
|
2606d73d93 | ||
|
ec42c09e06 | ||
|
4ab77c21ed | ||
|
b53384ab51 | ||
|
b026417efb | ||
|
7ffbd9d0e8 | ||
|
6fa5511670 | ||
|
c07ac38ab1 | ||
|
4174c2a4ab | ||
|
30236f9b36 | ||
|
7395aa4af8 | ||
|
2397b773d7 | ||
|
9a08f2df63 | ||
|
8245208b2b | ||
|
4ed2ee9adc | ||
|
06a353cfa0 | ||
|
dfe9e6b69f | ||
|
4d84eccac3 | ||
|
edb02a00ce | ||
|
5db19a0fb8 | ||
|
b62661c337 | ||
|
048ba3fd1e | ||
|
c8c3697fa0 | ||
|
8c3c5dfe33 | ||
|
92288603bf | ||
|
80c8b3ef62 | ||
|
127f3acce5 | ||
|
690c629e6d | ||
|
e328ea5c5d | ||
|
ed853eef1d | ||
|
05e77c924e | ||
|
923b031d06 | ||
|
7e29011d63 | ||
|
c5c5609fc6 | ||
|
9c263062e4 | ||
|
f39fd89fbd | ||
|
4e57a80265 | ||
|
27dc8059d7 | ||
|
dc2eae499f | ||
|
c518dcdc0b | ||
|
e6047f6693 | ||
|
96c4757cc6 | ||
|
60e564df51 | ||
|
a8b4e39733 | ||
|
d705626f4a | ||
|
c97284c15f | ||
|
df1f365c1e | ||
|
023c004842 | ||
|
2481f1c5d6 | ||
|
7dadecb5df | ||
|
6ef312b510 | ||
|
97f9fb4813 | ||
|
3d6c935308 | ||
|
fba9a30813 | ||
|
5f65fd5944 | ||
|
a26610bc7f | ||
|
264a53dacf | ||
|
0c96d4ffc2 | ||
|
843fa42692 | ||
|
b17dc5a0dd | ||
|
449a5f1670 | ||
|
06b8995861 | ||
|
2172d2e175 | ||
|
86291cbec4 | ||
|
66791b19f5 | ||
|
6689a1d35f | ||
|
91818ed80c | ||
|
86310aea4f | ||
|
a610e83f6e | ||
|
1449dc7bac |
@@ -37,7 +37,7 @@ int16_t lastclocktime;
|
||||
int8_t lastclockrate;
|
||||
|
||||
|
||||
#if WIFI_ON || ETHERNET_ON || defined(SERIAL1_COMMANDS) || defined(SERIAL2_COMMANDS) || defined(SERIAL3_COMMANDS)
|
||||
#if WIFI_ON || ETHERNET_ON || defined(SERIAL1_COMMANDS) || defined(SERIAL2_COMMANDS) || defined(SERIAL3_COMMANDS) || defined(SERIAL4_COMMANDS) || defined(SERIAL5_COMMANDS) || defined(SERIAL6_COMMANDS)
|
||||
// use a buffer to allow broadcast
|
||||
StringBuffer * CommandDistributor::broadcastBufferWriter=new StringBuffer();
|
||||
template<typename... Targs> void CommandDistributor::broadcastReply(clientType type, Targs... msg){
|
||||
@@ -248,6 +248,10 @@ void CommandDistributor::broadcastLoco(byte slot) {
|
||||
#endif
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastForgetLoco(int16_t loco) {
|
||||
broadcastReply(COMMAND_TYPE, F("<l %d 0 1 0>\n<- %d>\n"), loco,loco);
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastPower() {
|
||||
char pstr[] = "? x";
|
||||
for(byte t=0; t<TrackManager::MAX_TRACKS; t++)
|
||||
|
@@ -47,6 +47,7 @@ private:
|
||||
public :
|
||||
static void parse(byte clientId,byte* buffer, RingStream * ring);
|
||||
static void broadcastLoco(byte slot);
|
||||
static void broadcastForgetLoco(int16_t loco);
|
||||
static void broadcastSensor(int16_t id, bool value);
|
||||
static void broadcastTurnout(int16_t id, bool isClosed);
|
||||
static void broadcastTurntable(int16_t id, uint8_t position, bool moving);
|
||||
|
@@ -141,6 +141,23 @@ void setup()
|
||||
CommandDistributor::broadcastPower();
|
||||
}
|
||||
|
||||
/**************** for future reference
|
||||
void looptimer(unsigned long timeout, const FSH* message)
|
||||
{
|
||||
static unsigned long lasttimestamp = 0;
|
||||
unsigned long now = micros();
|
||||
if (timeout != 0) {
|
||||
unsigned long diff = now - lasttimestamp;
|
||||
if (diff > timeout) {
|
||||
DIAG(message);
|
||||
DIAG(F("DeltaT=%L"), diff);
|
||||
lasttimestamp = micros();
|
||||
return;
|
||||
}
|
||||
}
|
||||
lasttimestamp = now;
|
||||
}
|
||||
*********************************************/
|
||||
void loop()
|
||||
{
|
||||
// The main sketch has responsibilities during loop()
|
||||
@@ -148,14 +165,15 @@ void loop()
|
||||
// Responsibility 1: Handle DCC background processes
|
||||
// (loco reminders and power checks)
|
||||
DCC::loop();
|
||||
|
||||
|
||||
// Responsibility 2: handle any incoming commands on USB connection
|
||||
SerialManager::loop();
|
||||
|
||||
|
||||
// Responsibility 3: Optionally handle any incoming WiFi traffic
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
#if WIFI_ON
|
||||
WifiInterface::loop();
|
||||
|
||||
#endif //WIFI_ON
|
||||
#else //ARDUINO_ARCH_ESP32
|
||||
#ifndef WIFI_TASK_ON_CORE0
|
||||
|
20
DCC.cpp
20
DCC.cpp
@@ -271,6 +271,20 @@ uint32_t DCC::getFunctionMap(int cab) {
|
||||
return (reg<0)?0:speedTable[reg].functions;
|
||||
}
|
||||
|
||||
// saves DC frequency (0..3) in spare functions 29,30,31
|
||||
void DCC::setDCFreq(int cab,byte freq) {
|
||||
if (cab==0 || freq>3) return;
|
||||
auto reg=lookupSpeedTable(cab,true);
|
||||
// drop and replace F29,30,31 (top 3 bits)
|
||||
auto newFunctions=speedTable[reg].functions & 0x1FFFFFFFUL;
|
||||
if (freq==1) newFunctions |= (1UL<<29); // F29
|
||||
else if (freq==2) newFunctions |= (1UL<<30); // F30
|
||||
else if (freq==3) newFunctions |= (1UL<<31); // F31
|
||||
if (newFunctions==speedTable[reg].functions) return; // no change
|
||||
speedTable[reg].functions=newFunctions;
|
||||
CommandDistributor::broadcastLoco(reg);
|
||||
}
|
||||
|
||||
void DCC::setAccessory(int address, byte port, bool gate, byte onoff /*= 2*/) {
|
||||
// onoff is tristate:
|
||||
// 0 => send off packet
|
||||
@@ -728,11 +742,15 @@ void DCC::forgetLoco(int cab) { // removes any speed reminders for this loco
|
||||
if (reg>=0) {
|
||||
speedTable[reg].loco=0;
|
||||
setThrottle2(cab,1); // ESTOP if this loco still on track
|
||||
CommandDistributor::broadcastForgetLoco(cab);
|
||||
}
|
||||
}
|
||||
void DCC::forgetAllLocos() { // removes all speed reminders
|
||||
setThrottle2(0,1); // ESTOP all locos still on track
|
||||
for (int i=0;i<MAX_LOCOS;i++) speedTable[i].loco=0;
|
||||
for (int i=0;i<MAX_LOCOS;i++) {
|
||||
if (speedTable[i].loco) CommandDistributor::broadcastForgetLoco(speedTable[i].loco);
|
||||
speedTable[i].loco=0;
|
||||
}
|
||||
}
|
||||
|
||||
byte DCC::loopStatus=0;
|
||||
|
1
DCC.h
1
DCC.h
@@ -70,6 +70,7 @@ public:
|
||||
static void changeFn(int cab, int16_t functionNumber);
|
||||
static int8_t getFn(int cab, int16_t functionNumber);
|
||||
static uint32_t getFunctionMap(int cab);
|
||||
static void setDCFreq(int cab,byte freq);
|
||||
static void updateGroupflags(byte &flags, int16_t functionNumber);
|
||||
static void setAccessory(int address, byte port, bool gate, byte onoff = 2);
|
||||
static bool setExtendedAccessory(int16_t address, int16_t value, byte repeats=3);
|
||||
|
12
DCCACK.cpp
12
DCCACK.cpp
@@ -27,8 +27,8 @@
|
||||
#include "DCCWaveform.h"
|
||||
#include "TrackManager.h"
|
||||
|
||||
unsigned int DCCACK::minAckPulseDuration = 2000; // micros
|
||||
unsigned int DCCACK::maxAckPulseDuration = 20000; // micros
|
||||
unsigned long DCCACK::minAckPulseDuration = 2000; // micros
|
||||
unsigned long DCCACK::maxAckPulseDuration = 20000; // micros
|
||||
|
||||
MotorDriver * DCCACK::progDriver=NULL;
|
||||
ackOp const * DCCACK::ackManagerProg;
|
||||
@@ -50,8 +50,8 @@ volatile uint8_t DCCACK::numAckSamples=0;
|
||||
uint8_t DCCACK::trailingEdgeCounter=0;
|
||||
|
||||
|
||||
unsigned int DCCACK::ackPulseDuration; // micros
|
||||
unsigned long DCCACK::ackPulseStart; // micros
|
||||
unsigned long DCCACK::ackPulseDuration; // micros
|
||||
unsigned long DCCACK::ackPulseStart; // micros
|
||||
volatile bool DCCACK::ackDetected;
|
||||
unsigned long DCCACK::ackCheckStart; // millis
|
||||
volatile bool DCCACK::ackPending;
|
||||
@@ -127,7 +127,7 @@ bool DCCACK::checkResets(uint8_t numResets) {
|
||||
void DCCACK::setAckBaseline() {
|
||||
int baseline=progDriver->getCurrentRaw();
|
||||
ackThreshold= baseline + progDriver->mA2raw(ackLimitmA);
|
||||
if (Diag::ACK) DIAG(F("ACK baseline=%d/%dmA Threshold=%d/%dmA Duration between %uus and %uus"),
|
||||
if (Diag::ACK) DIAG(F("ACK baseline=%d/%dmA Threshold=%d/%dmA Duration between %lus and %lus"),
|
||||
baseline,progDriver->raw2mA(baseline),
|
||||
ackThreshold,progDriver->raw2mA(ackThreshold),
|
||||
minAckPulseDuration, maxAckPulseDuration);
|
||||
@@ -146,7 +146,7 @@ void DCCACK::setAckPending() {
|
||||
|
||||
byte DCCACK::getAck() {
|
||||
if (ackPending) return (2); // still waiting
|
||||
if (Diag::ACK) DIAG(F("%S after %dmS max=%d/%dmA pulse=%uuS samples=%d gaps=%d"),ackDetected?F("ACK"):F("NO-ACK"), ackCheckDuration,
|
||||
if (Diag::ACK) DIAG(F("%S after %dmS max=%d/%dmA pulse=%luS samples=%d gaps=%d"),ackDetected?F("ACK"):F("NO-ACK"), ackCheckDuration,
|
||||
ackMaxCurrent,progDriver->raw2mA(ackMaxCurrent), ackPulseDuration, numAckSamples, numAckGaps);
|
||||
if (ackDetected) return (1); // Yes we had an ack
|
||||
return(0); // pending set off but not detected means no ACK.
|
||||
|
10
DCCACK.h
10
DCCACK.h
@@ -79,10 +79,10 @@ class DCCACK {
|
||||
static inline void setAckLimit(int mA) {
|
||||
ackLimitmA = mA;
|
||||
}
|
||||
static inline void setMinAckPulseDuration(unsigned int i) {
|
||||
static inline void setMinAckPulseDuration(unsigned long i) {
|
||||
minAckPulseDuration = i;
|
||||
}
|
||||
static inline void setMaxAckPulseDuration(unsigned int i) {
|
||||
static inline void setMaxAckPulseDuration(unsigned long i) {
|
||||
maxAckPulseDuration = i;
|
||||
}
|
||||
|
||||
@@ -126,11 +126,11 @@ class DCCACK {
|
||||
static unsigned long ackCheckStart; // millis
|
||||
static unsigned int ackCheckDuration; // millis
|
||||
|
||||
static unsigned int ackPulseDuration; // micros
|
||||
static unsigned long ackPulseDuration; // micros
|
||||
static unsigned long ackPulseStart; // micros
|
||||
|
||||
static unsigned int minAckPulseDuration ; // micros
|
||||
static unsigned int maxAckPulseDuration ; // micros
|
||||
static unsigned long minAckPulseDuration ; // micros
|
||||
static unsigned long maxAckPulseDuration ; // micros
|
||||
static MotorDriver* progDriver;
|
||||
static volatile uint8_t numAckGaps;
|
||||
static volatile uint8_t numAckSamples;
|
||||
|
123
DCCEXParser.cpp
123
DCCEXParser.cpp
@@ -2,7 +2,7 @@
|
||||
* © 2022 Paul M Antoine
|
||||
* © 2021 Neil McKechnie
|
||||
* © 2021 Mike S
|
||||
* © 2021 Herb Morton
|
||||
* © 2021-2024 Herb Morton
|
||||
* © 2020-2023 Harald Barth
|
||||
* © 2020-2021 M Steve Todd
|
||||
* © 2020-2021 Fred Decker
|
||||
@@ -72,7 +72,7 @@ Once a new OPCODE is decided upon, update this list.
|
||||
M, Write DCC packet
|
||||
n, Reserved for SensorCam
|
||||
N, Reserved for Sensorcam
|
||||
o,
|
||||
o, Neopixel driver (see also IO_NeoPixel.h)
|
||||
O, Output broadcast
|
||||
p, Broadcast power state
|
||||
P, Write DCC packet
|
||||
@@ -117,6 +117,9 @@ Once a new OPCODE is decided upon, update this list.
|
||||
#include "Turntables.h"
|
||||
#include "version.h"
|
||||
#include "KeywordHasher.h"
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
#include "WifiESP32.h"
|
||||
#endif
|
||||
|
||||
// This macro can't be created easily as a portable function because the
|
||||
// flashlist requires a far pointer for high flash access.
|
||||
@@ -140,12 +143,12 @@ byte DCCEXParser::stashTarget=0;
|
||||
// Non-DCC things like turnouts, pins and sensors are handled in additional JMRI interface classes.
|
||||
|
||||
|
||||
int16_t DCCEXParser::splitValues(int16_t result[MAX_COMMAND_PARAMS], const byte *cmd, bool usehex)
|
||||
int16_t DCCEXParser::splitValues(int16_t result[MAX_COMMAND_PARAMS], byte *cmd, bool usehex)
|
||||
{
|
||||
byte state = 1;
|
||||
byte parameterCount = 0;
|
||||
int16_t runningValue = 0;
|
||||
const byte *remainingCmd = cmd + 1; // skips the opcode
|
||||
byte *remainingCmd = cmd + 1; // skips the opcode
|
||||
bool signNegative = false;
|
||||
|
||||
// clear all parameters in case not enough found
|
||||
@@ -155,7 +158,6 @@ int16_t DCCEXParser::splitValues(int16_t result[MAX_COMMAND_PARAMS], const byte
|
||||
while (parameterCount < MAX_COMMAND_PARAMS)
|
||||
{
|
||||
byte hot = *remainingCmd;
|
||||
|
||||
switch (state)
|
||||
{
|
||||
|
||||
@@ -169,7 +171,22 @@ int16_t DCCEXParser::splitValues(int16_t result[MAX_COMMAND_PARAMS], const byte
|
||||
state = 2;
|
||||
continue;
|
||||
|
||||
case 2: // checking sign
|
||||
case 2: // checking sign or quoted string
|
||||
#ifdef HAS_ENOUGH_MEMORY
|
||||
if (hot == '"') {
|
||||
// this inserts an extra parameter 0x7777 in front
|
||||
// of each string parameter as a marker that can
|
||||
// be checked that a string parameter follows
|
||||
// This clashes of course with the real value
|
||||
// 0x7777 which we hope is used seldom
|
||||
result[parameterCount] = (int16_t)0x7777;
|
||||
parameterCount++;
|
||||
result[parameterCount] = (int16_t)(remainingCmd - cmd + 1);
|
||||
parameterCount++;
|
||||
state = 4;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
signNegative = false;
|
||||
runningValue = 0;
|
||||
state = 3;
|
||||
@@ -200,6 +217,16 @@ int16_t DCCEXParser::splitValues(int16_t result[MAX_COMMAND_PARAMS], const byte
|
||||
parameterCount++;
|
||||
state = 1;
|
||||
continue;
|
||||
#ifdef HAS_ENOUGH_MEMORY
|
||||
case 4: // skipover text
|
||||
if (hot == '\0') // We did run to end of buffer without finding the "
|
||||
return -1;
|
||||
if (hot == '"') {
|
||||
*remainingCmd = '\0'; // overwrite " in command buffer with the end-of-string
|
||||
state = 1;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
remainingCmd++;
|
||||
}
|
||||
@@ -394,7 +421,36 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
return;
|
||||
break;
|
||||
|
||||
case 'z': // direct pin manipulation
|
||||
#ifndef IO_NO_HAL
|
||||
case 'o': // Neopixel pin manipulation
|
||||
if (p[0]==0) break;
|
||||
{
|
||||
VPIN vpin=p[0]>0 ? p[0]:-p[0];
|
||||
bool setON=p[0]>0;
|
||||
if (params==1) { // <o [-]vpin>
|
||||
IODevice::write(vpin,setON);
|
||||
return;
|
||||
}
|
||||
if (params==2) { // <o [-]vpin count>
|
||||
IODevice::writeRange(vpin,setON,p[1]);
|
||||
return;
|
||||
}
|
||||
if (params==4 || params==5) { // <z [-]vpin r g b [count]>
|
||||
auto count=p[4]?p[4]:1;
|
||||
if (p[1]<0 || p[1]>0xFF) break;
|
||||
if (p[2]<0 || p[2]>0xFF) break;
|
||||
if (p[3]<0 || p[3]>0xFF) break;
|
||||
// strange parameter mangling... see IO_NeoPixel.h NeoPixel::_writeAnalogue
|
||||
int colour_RG=(p[1]<<8) | p[2];
|
||||
uint16_t colour_B=p[3];
|
||||
IODevice::writeAnalogueRange(vpin,colour_RG,setON,colour_B,count);
|
||||
return;
|
||||
}
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
case 'z': // direct pin manipulation
|
||||
if (p[0]==0) break;
|
||||
if (params==1) { // <z vpin | -vpin>
|
||||
if (p[0]>0) IODevice::write(p[0],HIGH);
|
||||
@@ -521,7 +577,7 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
{
|
||||
if (params > 1) break;
|
||||
if (params==0) { // All
|
||||
TrackManager::setTrackPower(TRACK_MODE_ALL, POWERMODE::ON);
|
||||
TrackManager::setTrackPower(TRACK_ALL, POWERMODE::ON);
|
||||
}
|
||||
if (params==1) {
|
||||
if (p[0]=="MAIN"_hk) { // <1 MAIN>
|
||||
@@ -554,7 +610,7 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
if (params > 1) break;
|
||||
if (params==0) { // All
|
||||
TrackManager::setJoin(false);
|
||||
TrackManager::setTrackPower(TRACK_MODE_ALL, POWERMODE::OFF);
|
||||
TrackManager::setTrackPower(TRACK_ALL, POWERMODE::OFF);
|
||||
}
|
||||
if (params==1) {
|
||||
if (p[0]=="MAIN"_hk) { // <0 MAIN>
|
||||
@@ -563,6 +619,7 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
}
|
||||
#ifndef DISABLE_PROG
|
||||
else if (p[0]=="PROG"_hk) { // <0 PROG>
|
||||
TrackManager::setJoin(false);
|
||||
TrackManager::progTrackBoosted=false; // Prog track boost mode will not outlive prog track off
|
||||
TrackManager::setTrackPower(TRACK_MODE_PROG, POWERMODE::OFF);
|
||||
}
|
||||
@@ -615,9 +672,22 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
StringFormatter::send(stream, F("\n"));
|
||||
return;
|
||||
case 'C': // CONFIG <C [params]>
|
||||
if (parseC(stream, params, p))
|
||||
return;
|
||||
break;
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
// currently this only works on ESP32
|
||||
#if defined(HAS_ENOUGH_MEMORY)
|
||||
if (p[0] == "WIFI"_hk) { // <C WIFI SSID PASSWORD>
|
||||
if (params != 5) // the 5 params 0 to 4 are (kinda): WIFI_hk 0x7777 &SSID 0x7777 &PASSWORD
|
||||
break;
|
||||
if (p[1] == 0x7777 && p[3] == 0x7777) {
|
||||
WifiESP::setup((const char*)(com + p[2]), (const char*)(com + p[4]), WIFI_HOSTNAME, IP_PORT, WIFI_CHANNEL, WIFI_FORCE_AP);
|
||||
}
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
#endif //ESP32
|
||||
if (parseC(stream, params, p))
|
||||
return;
|
||||
break;
|
||||
#ifndef DISABLE_DIAG
|
||||
case 'D': // DIAG <D [params]>
|
||||
if (parseD(stream, params, p))
|
||||
@@ -641,6 +711,13 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
|
||||
case 'F': // New command to call the new Loco Function API <F cab func 1|0>
|
||||
if(params!=3) break;
|
||||
|
||||
if (p[1]=="DCFREQ"_hk) { // <F cab DCFREQ 0..3>
|
||||
if (p[2]<0 || p[2]>3) break;
|
||||
DCC::setDCFreq(p[0],p[2]);
|
||||
return;
|
||||
}
|
||||
|
||||
if (Diag::CMD)
|
||||
DIAG(F("Setting loco %d F%d %S"), p[0], p[1], p[2] ? F("ON") : F("OFF"));
|
||||
if (DCC::setFn(p[0], p[1], p[2] == 1)) return;
|
||||
@@ -1073,15 +1150,24 @@ bool DCCEXParser::parseC(Print *stream, int16_t params, int16_t p[]) {
|
||||
#ifndef DISABLE_PROG
|
||||
case "ACK"_hk: // <D ACK ON/OFF> <D ACK [LIMIT|MIN|MAX|RETRY] Value>
|
||||
if (params >= 3) {
|
||||
long duration;
|
||||
if (p[1] == "LIMIT"_hk) {
|
||||
DCCACK::setAckLimit(p[2]);
|
||||
LCD(1, F("Ack Limit=%dmA"), p[2]); // <D ACK LIMIT 42>
|
||||
LCD(1, F("Ack Limit=%dmA"), p[2]); // <D ACK LIMIT 42>
|
||||
} else if (p[1] == "MIN"_hk) {
|
||||
DCCACK::setMinAckPulseDuration(p[2]);
|
||||
LCD(0, F("Ack Min=%uus"), p[2]); // <D ACK MIN 1500>
|
||||
if (params == 4 && p[3] == "MS"_hk)
|
||||
duration = p[2] * 1000L;
|
||||
else
|
||||
duration = p[2];
|
||||
DCCACK::setMinAckPulseDuration(duration);
|
||||
LCD(0, F("Ack Min=%lus"), duration); // <D ACK MIN 1500>
|
||||
} else if (p[1] == "MAX"_hk) {
|
||||
DCCACK::setMaxAckPulseDuration(p[2]);
|
||||
LCD(0, F("Ack Max=%uus"), p[2]); // <D ACK MAX 9000>
|
||||
if (params == 4 && p[3] == "MS"_hk) // <D ACK MAX 80 MS>
|
||||
duration = p[2] * 1000L;
|
||||
else
|
||||
duration = p[2];
|
||||
DCCACK::setMaxAckPulseDuration(duration);
|
||||
LCD(0, F("Ack Max=%lus"), duration); // <D ACK MAX 9000>
|
||||
} else if (p[1] == "RETRY"_hk) {
|
||||
if (p[2] >255) p[2]=3;
|
||||
LCD(0, F("Ack Retry=%d Sum=%d"), p[2], DCCACK::setAckRetry(p[2])); // <D ACK RETRY 2>
|
||||
@@ -1094,8 +1180,7 @@ bool DCCEXParser::parseC(Print *stream, int16_t params, int16_t p[]) {
|
||||
}
|
||||
return true;
|
||||
#endif
|
||||
|
||||
default: // invalid/unknown
|
||||
default: // invalid/unknown
|
||||
break;
|
||||
}
|
||||
return false;
|
||||
|
@@ -43,7 +43,7 @@ struct DCCEXParser
|
||||
private:
|
||||
|
||||
static const int16_t MAX_BUFFER=50; // longest command sent in
|
||||
static int16_t splitValues( int16_t result[MAX_COMMAND_PARAMS], const byte * command, bool usehex);
|
||||
static int16_t splitValues( int16_t result[MAX_COMMAND_PARAMS], byte * command, bool usehex);
|
||||
|
||||
static bool parseT(Print * stream, int16_t params, int16_t p[]);
|
||||
static bool parseZ(Print * stream, int16_t params, int16_t p[]);
|
||||
|
19
DCCRMT.cpp
19
DCCRMT.cpp
@@ -17,6 +17,25 @@
|
||||
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* RMT has "channels" which us FIFO RAM where you place what you want to send
|
||||
* or receive. Channels can be merged to get more words per channel.
|
||||
*
|
||||
* WROOM: 8 channels total of 512 words, 64 words per channel. We use currently
|
||||
* channel 0+1 for 128 words for DCC MAIN and 2+3 for DCC PROG.
|
||||
*
|
||||
* S3: 8 channels total of 384 words. 4 channels dedicated for TX and 4 channels
|
||||
* dedicated for RX. 48 words per channel. So for TX there are 4 channels and we
|
||||
* could use them with 96 words for MAIN and PROG if DCC data does fit in there.
|
||||
*
|
||||
* C3: 4 channels total of 192 words. As we do not use RX we can use all for TX
|
||||
* so the situation is the same as for the -S3
|
||||
*
|
||||
* C6, H2: 4 channels total of 192 words. 2 channels dedictaed for TX and
|
||||
* 2 channels dedicated for RX. Half RMT capacity compared to the C3.
|
||||
*
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
#include "defines.h"
|
||||
#include "DIAG.h"
|
||||
|
@@ -185,8 +185,10 @@ int DCCTimer::freeMemory() {
|
||||
}
|
||||
|
||||
void DCCTimer::reset() {
|
||||
wdt_enable( WDTO_15MS); // set Arduino watchdog timer for 15ms
|
||||
delay(50); // wait for the prescaller time to expire
|
||||
// 250ms chosen to circumwent bootloader bug which
|
||||
// hangs at too short timepout (like 15ms)
|
||||
wdt_enable( WDTO_250MS); // set Arduino watchdog timer for 250ms
|
||||
delay(500); // wait for it to happen
|
||||
|
||||
}
|
||||
|
||||
|
@@ -76,8 +76,13 @@ int DCCTimer::freeMemory() {
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
|
||||
#include "esp_idf_version.h"
|
||||
#if ESP_IDF_VERSION_MAJOR > 4
|
||||
#error "DCC-EX does not support compiling with IDF version 5.0 or later. Downgrade your ESP32 library to a version that contains IDE version 4. Arduino ESP32 library 3.0.0 is too new. Downgrade to one of 2.0.9 to 2.0.17"
|
||||
#endif
|
||||
|
||||
#include "DIAG.h"
|
||||
#include <driver/adc.h>
|
||||
#include <soc/sens_reg.h>
|
||||
@@ -292,7 +297,12 @@ void DCCTimer::DCCEXInrushControlOn(uint8_t pin, int duty, bool inverted) {
|
||||
int ADCee::init(uint8_t pin) {
|
||||
pinMode(pin, ANALOG);
|
||||
adc1_config_width(ADC_WIDTH_BIT_12);
|
||||
// Espressif deprecated ADC_ATTEN_DB_11 somewhere between 2.0.9 and 2.0.17
|
||||
#ifdef ADC_ATTEN_11db
|
||||
adc1_config_channel_atten(pinToADC1Channel(pin),ADC_ATTEN_11db);
|
||||
#else
|
||||
adc1_config_channel_atten(pinToADC1Channel(pin),ADC_ATTEN_DB_11);
|
||||
#endif
|
||||
return adc1_get_raw(pinToADC1Channel(pin));
|
||||
}
|
||||
int16_t ADCee::ADCmax() {
|
||||
|
@@ -36,7 +36,20 @@
|
||||
#include "DIAG.h"
|
||||
#include <wiring_private.h>
|
||||
|
||||
#if defined(ARDUINO_NUCLEO_F401RE) || defined(ARDUINO_NUCLEO_F411RE)
|
||||
#if defined(ARDUINO_NUCLEO_F401RE)
|
||||
// Nucleo-64 boards don't have additional serial ports defined by default
|
||||
// Serial1 is available on the F401RE, but not hugely convenient.
|
||||
// Rx pin on PB7 is useful, but all the Tx pins map to Arduino digital pins, specifically:
|
||||
// PA9 == D8
|
||||
// PB6 == D10
|
||||
// of which D8 is needed by the standard and EX8874 motor shields. D10 would be used if a second
|
||||
// EX8874 is stacked. So only disable this if using a second motor shield.
|
||||
HardwareSerial Serial1(PB7, PB6); // Rx=PB7, Tx=PB6 -- CN7 pin 17 and CN10 pin 17
|
||||
// Serial2 is defined to use USART2 by default, but is in fact used as the diag console
|
||||
// via the debugger on the Nucleo-64. It is therefore unavailable for other DCC-EX uses like WiFi, DFPlayer, etc.
|
||||
// Let's define Serial6 as an additional serial port (the only other option for the F401RE)
|
||||
HardwareSerial Serial6(PA12, PA11); // Rx=PA12, Tx=PA11 -- CN10 pins 12 and 14 - F401RE
|
||||
#elif defined(ARDUINO_NUCLEO_F411RE)
|
||||
// Nucleo-64 boards don't have additional serial ports defined by default
|
||||
HardwareSerial Serial1(PB7, PA15); // Rx=PB7, Tx=PA15 -- CN7 pins 17 and 21 - F411RE
|
||||
// Serial2 is defined to use USART2 by default, but is in fact used as the diag console
|
||||
@@ -54,12 +67,12 @@ HardwareSerial Serial3(PC11, PC10); // Rx=PC11, Tx=PC10 -- USART3 - F446RE
|
||||
HardwareSerial Serial5(PD2, PC12); // Rx=PD2, Tx=PC12 -- UART5 - F446RE
|
||||
// On the F446RE, Serial4 and Serial6 also use pins we can't readily map while using the Arduino pins
|
||||
#elif defined(ARDUINO_NUCLEO_F412ZG) || defined(ARDUINO_NUCLEO_F413ZH) || defined(ARDUINO_NUCLEO_F446ZE) || \
|
||||
defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F439ZI)
|
||||
defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F439ZI) || defined(ARDUINO_NUCLEO_F4X9ZI)
|
||||
// Nucleo-144 boards don't have Serial1 defined by default
|
||||
HardwareSerial Serial6(PG9, PG14); // Rx=PG9, Tx=PG14 -- USART6
|
||||
HardwareSerial Serial5(PD2, PC12); // Rx=PD2, Tx=PC12 -- UART5
|
||||
#if !defined(ARDUINO_NUCLEO_F412ZG)
|
||||
HardwareSerial Serial2(PD6, PD5); // Rx=PD6, Tx=PD5 -- UART5
|
||||
HardwareSerial Serial2(PD6, PD5); // Rx=PD6, Tx=PD5 -- UART2
|
||||
#if !defined(ARDUINO_NUCLEO_F412ZG) // F412ZG does not have UART5
|
||||
HardwareSerial Serial5(PD2, PC12); // Rx=PD2, Tx=PC12 -- UART5
|
||||
#endif
|
||||
// Serial3 is defined to use USART3 by default, but is in fact used as the diag console
|
||||
// via the debugger on the Nucleo-144. It is therefore unavailable for other DCC-EX uses like WiFi, DFPlayer, etc.
|
||||
@@ -315,7 +328,7 @@ void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t frequency
|
||||
if (pin_timer[pin] != NULL)
|
||||
{
|
||||
pin_timer[pin]->setPWM(pin_channel[pin], pin, frequency, 0); // set frequency in Hertz, 0% dutycycle
|
||||
DIAG(F("DCCEXanalogWriteFrequency::Pin %d on Timer %d, frequency %d"), pin, pin_channel[pin], frequency);
|
||||
DIAG(F("DCCEXanalogWriteFrequency::Pin %d on Timer Channel %d, frequency %d"), pin, pin_channel[pin], frequency);
|
||||
}
|
||||
else
|
||||
DIAG(F("DCCEXanalogWriteFrequency::failed to allocate HardwareTimer instance!"));
|
||||
|
253
EXRAIL2.cpp
253
EXRAIL2.cpp
@@ -1,4 +1,5 @@
|
||||
/*
|
||||
* © 2024 Paul M. Antoine
|
||||
* © 2021 Neil McKechnie
|
||||
* © 2021-2023 Harald Barth
|
||||
* © 2020-2023 Chris Harlow
|
||||
@@ -72,6 +73,7 @@ RMFT2 * RMFT2::pausingTask=NULL; // Task causing a PAUSE.
|
||||
byte RMFT2::flags[MAX_FLAGS];
|
||||
Print * RMFT2::LCCSerial=0;
|
||||
LookList * RMFT2::routeLookup=NULL;
|
||||
LookList * RMFT2::signalLookup=NULL;
|
||||
LookList * RMFT2::onThrowLookup=NULL;
|
||||
LookList * RMFT2::onCloseLookup=NULL;
|
||||
LookList * RMFT2::onActivateLookup=NULL;
|
||||
@@ -206,16 +208,28 @@ LookList* RMFT2::LookListLoader(OPCODE op1, OPCODE op2, OPCODE op3) {
|
||||
// Second pass startup, define any turnouts or servos, set signals red
|
||||
// add sequences onRoutines to the lookups
|
||||
if (compileFeatures & FEATURE_SIGNAL) {
|
||||
|
||||
onRedLookup=LookListLoader(OPCODE_ONRED);
|
||||
onAmberLookup=LookListLoader(OPCODE_ONAMBER);
|
||||
onGreenLookup=LookListLoader(OPCODE_ONGREEN);
|
||||
for (int sigslot=0;;sigslot++) {
|
||||
int16_t sighandle=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigslot*8);
|
||||
if (sighandle==0) break; // end of signal list
|
||||
VPIN sigid = sighandle & SIGNAL_ID_MASK;
|
||||
doSignal(sigid, SIGNAL_RED);
|
||||
}
|
||||
}
|
||||
// Load the signal lookup with slot numbers in the signal table
|
||||
int signalCount=0;
|
||||
for (int16_t slot=0;;slot++) {
|
||||
SIGNAL_DEFINITION signal=getSignalSlot(slot);
|
||||
DIAG(F("Signal s=%d id=%d t=%d"),slot,signal.id,signal.type);
|
||||
if (signal.type==sigtypeNoMoreSignals) break;
|
||||
if (signal.type==sigtypeContinuation) continue;
|
||||
signalCount++;
|
||||
}
|
||||
signalLookup=new LookList(signalCount);
|
||||
for (int16_t slot=0;;slot++) {
|
||||
SIGNAL_DEFINITION signal=getSignalSlot(slot);
|
||||
if (signal.type==sigtypeNoMoreSignals) break;
|
||||
if (signal.type==sigtypeContinuation) continue;
|
||||
signalLookup->add(signal.id,slot);
|
||||
doSignal(signal.id, SIGNAL_RED);
|
||||
}
|
||||
}
|
||||
|
||||
int progCounter;
|
||||
for (progCounter=0;; SKIPOP){
|
||||
@@ -227,7 +241,6 @@ LookList* RMFT2::LookListLoader(OPCODE op1, OPCODE op2, OPCODE op3) {
|
||||
case OPCODE_AT:
|
||||
case OPCODE_ATTIMEOUT2:
|
||||
case OPCODE_AFTER:
|
||||
case OPCODE_AFTEROVERLOAD:
|
||||
case OPCODE_IF:
|
||||
case OPCODE_IFNOT: {
|
||||
int16_t pin = (int16_t)operand;
|
||||
@@ -478,10 +491,15 @@ bool RMFT2::skipIfBlock() {
|
||||
|
||||
|
||||
/* static */ void RMFT2::readLocoCallback(int16_t cv) {
|
||||
if (cv <= 0) {
|
||||
DIAG(F("CV read error"));
|
||||
progtrackLocoId = -1;
|
||||
return;
|
||||
}
|
||||
if (cv & LONG_ADDR_MARKER) { // maker bit indicates long addr
|
||||
progtrackLocoId = cv ^ LONG_ADDR_MARKER; // remove marker bit to get real long addr
|
||||
if (progtrackLocoId <= HIGHEST_SHORT_ADDR ) { // out of range for long addr
|
||||
DIAG(F("Long addr %d <= %d unsupported\n"), progtrackLocoId, HIGHEST_SHORT_ADDR);
|
||||
DIAG(F("Long addr %d <= %d unsupported"), progtrackLocoId, HIGHEST_SHORT_ADDR);
|
||||
progtrackLocoId = -1;
|
||||
}
|
||||
} else {
|
||||
@@ -628,14 +646,16 @@ void RMFT2::loop2() {
|
||||
skipIf=blinkState!=at_timeout;
|
||||
break;
|
||||
|
||||
case OPCODE_AFTER: // waits for sensor to hit and then remain off for 0.5 seconds. (must come after an AT operation)
|
||||
case OPCODE_AFTER: // waits for sensor to hit and then remain off for x mS.
|
||||
// Note, this must come after an AT operation, which is
|
||||
// automatically inserted by the AFTER macro.
|
||||
if (readSensor(operand)) {
|
||||
// reset timer to half a second and keep waiting
|
||||
// reset timer and keep waiting
|
||||
waitAfter=millis();
|
||||
delayMe(50);
|
||||
return;
|
||||
}
|
||||
if (millis()-waitAfter < 500 ) return;
|
||||
if (millis()-waitAfter < getOperand(1) ) return;
|
||||
break;
|
||||
|
||||
case OPCODE_AFTEROVERLOAD: // waits for the power to be turned back on - either by power routine or button
|
||||
@@ -716,41 +736,7 @@ void RMFT2::loop2() {
|
||||
|
||||
case OPCODE_SETFREQ:
|
||||
// Frequency is default 0, or 1, 2,3
|
||||
//if (loco) DCC::setFn(loco,operand,true);
|
||||
switch (operand) {
|
||||
case 0: // default - all F-s off
|
||||
if (loco) {
|
||||
DCC::setFn(loco,29,false);
|
||||
DCC::setFn(loco,30,false);
|
||||
DCC::setFn(loco,31,false);
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if (loco) {
|
||||
DCC::setFn(loco,29,true);
|
||||
DCC::setFn(loco,30,false);
|
||||
DCC::setFn(loco,31,false);
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
if (loco) {
|
||||
DCC::setFn(loco,29,false);
|
||||
DCC::setFn(loco,30,true);
|
||||
DCC::setFn(loco,31,false);
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if (loco) {
|
||||
DCC::setFn(loco,29,false);
|
||||
DCC::setFn(loco,30,false);
|
||||
DCC::setFn(loco,31,true);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
; // do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
DCC::setDCFreq(loco,operand);
|
||||
break;
|
||||
|
||||
case OPCODE_RESUME:
|
||||
@@ -953,11 +939,10 @@ void RMFT2::loop2() {
|
||||
delayMe(100);
|
||||
return; // still waiting for callback
|
||||
}
|
||||
if (progtrackLocoId<0) {
|
||||
kill(F("No Loco Found"),progtrackLocoId);
|
||||
return; // still waiting for callback
|
||||
}
|
||||
|
||||
// At failed read will result in loco == -1
|
||||
// which is intended so it can be checked
|
||||
// from within EXRAIL
|
||||
loco=progtrackLocoId;
|
||||
speedo=0;
|
||||
forward=true;
|
||||
@@ -1000,6 +985,14 @@ void RMFT2::loop2() {
|
||||
if ((compileFeatures & FEATURE_LCC) && LCCSerial)
|
||||
StringFormatter::send(LCCSerial,F("<L x%h>"),(uint16_t)operand);
|
||||
break;
|
||||
|
||||
case OPCODE_ACON: // MERG adapter
|
||||
case OPCODE_ACOF:
|
||||
if ((compileFeatures & FEATURE_LCC) && LCCSerial)
|
||||
StringFormatter::send(LCCSerial,F("<L x%c%h%h>"),
|
||||
opcode==OPCODE_ACON?'0':'1',
|
||||
(uint16_t)operand,getOperand(progCounter,1));
|
||||
break;
|
||||
|
||||
case OPCODE_LCCX: // long form LCC
|
||||
if ((compileFeatures & FEATURE_LCC) && LCCSerial)
|
||||
@@ -1021,8 +1014,18 @@ void RMFT2::loop2() {
|
||||
return;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
#ifndef IO_NO_HAL
|
||||
case OPCODE_NEOPIXEL:
|
||||
// OPCODE_NEOPIXEL,V([-]vpin),OPCODE_PAD,V(colour_RG),OPCODE_PAD,V(colour_B),OPCODE_PAD,V(count)
|
||||
{
|
||||
VPIN vpin=operand>0?operand:-operand;
|
||||
auto count=getOperand(3);
|
||||
killBlinkOnVpin(vpin,count);
|
||||
IODevice::writeAnalogueRange(vpin,getOperand(1),operand>0,getOperand(2),count);
|
||||
}
|
||||
break;
|
||||
|
||||
case OPCODE_WAITFORTT: // OPCODE_WAITFOR,V(turntable_id)
|
||||
if (Turntable::ttMoving(operand)) {
|
||||
delayMe(100);
|
||||
@@ -1088,6 +1091,8 @@ void RMFT2::loop2() {
|
||||
case OPCODE_PINTURNOUT: // Turnout definition ignored at runtime
|
||||
case OPCODE_ONCLOSE: // Turnout event catchers ignored here
|
||||
case OPCODE_ONLCC: // LCC event catchers ignored here
|
||||
case OPCODE_ONACON: // MERG event catchers ignored here
|
||||
case OPCODE_ONACOF: // MERG event catchers ignored here
|
||||
case OPCODE_ONTHROW:
|
||||
case OPCODE_ONACTIVATE: // Activate event catchers ignored here
|
||||
case OPCODE_ONDEACTIVATE:
|
||||
@@ -1142,21 +1147,11 @@ void RMFT2::kill(const FSH * reason, int operand) {
|
||||
delete this;
|
||||
}
|
||||
|
||||
int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
for (int sigslot=0;;sigslot++) {
|
||||
int16_t sighandle=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigslot*8);
|
||||
if (sighandle==0) { // end of signal list
|
||||
DIAG(F("EXRAIL Signal %d not defined"), id);
|
||||
return -1;
|
||||
}
|
||||
VPIN sigid = sighandle & SIGNAL_ID_MASK;
|
||||
// sigid is the signal id used in RED/AMBER/GREEN macro
|
||||
// for a LED signal it will be same as redpin
|
||||
// but for a servo signal it will also have SERVO_SIGNAL_FLAG set.
|
||||
|
||||
if (sigid != id) continue; // keep looking
|
||||
return sigslot; // relative slot in signals table
|
||||
}
|
||||
SIGNAL_DEFINITION RMFT2::getSignalSlot(int16_t slot) {
|
||||
SIGNAL_DEFINITION signal;
|
||||
COPYHIGHFLASH(&signal,SignalDefinitions,slot*sizeof(SIGNAL_DEFINITION),sizeof(SIGNAL_DEFINITION));
|
||||
return signal;
|
||||
}
|
||||
|
||||
/* static */ void RMFT2::doSignal(int16_t id,char rag) {
|
||||
@@ -1169,81 +1164,97 @@ int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
else if (rag==SIGNAL_GREEN) onGreenLookup->handleEvent(F("GREEN"),id);
|
||||
else onAmberLookup->handleEvent(F("AMBER"),id);
|
||||
|
||||
int16_t sigslot=getSignalSlot(id);
|
||||
auto sigslot=signalLookup->find(id);
|
||||
if (sigslot<0) return;
|
||||
|
||||
// keep track of signal state
|
||||
setFlag(sigslot,rag,SIGNAL_MASK);
|
||||
|
||||
// Correct signal definition found, get the rag values
|
||||
int16_t sigpos=sigslot*8;
|
||||
int16_t sighandle=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos);
|
||||
VPIN redpin=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+2);
|
||||
VPIN amberpin=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+4);
|
||||
VPIN greenpin=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+6);
|
||||
//if (diag) DIAG(F("signal %d %d %d %d %d"),sigid,id,redpin,amberpin,greenpin);
|
||||
|
||||
VPIN sigtype=sighandle & ~SIGNAL_ID_MASK;
|
||||
VPIN sigid = sighandle & SIGNAL_ID_MASK;
|
||||
|
||||
if (sigtype == SERVO_SIGNAL_FLAG) {
|
||||
// A servo signal, the pin numbers are actually servo positions
|
||||
// Note, setting a signal to a zero position has no effect.
|
||||
int16_t servopos= rag==SIGNAL_RED? redpin: (rag==SIGNAL_GREEN? greenpin : amberpin);
|
||||
auto signal=getSignalSlot(sigslot);
|
||||
|
||||
switch (signal.type) {
|
||||
case sigtypeSERVO:
|
||||
{
|
||||
auto servopos = rag==SIGNAL_RED? signal.redpin: (rag==SIGNAL_GREEN? signal.greenpin : signal.amberpin);
|
||||
//if (diag) DIAG(F("sigA %d %d"),id,servopos);
|
||||
if (servopos!=0) IODevice::writeAnalogue(id,servopos,PCA9685::Bounce);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (sigtype== DCC_SIGNAL_FLAG) {
|
||||
case sigtypeDCC:
|
||||
{
|
||||
// redpin,amberpin are the DCC addr,subaddr
|
||||
DCC::setAccessory(redpin,amberpin, rag!=SIGNAL_RED);
|
||||
DCC::setAccessory(signal.redpin,signal.amberpin, rag!=SIGNAL_RED);
|
||||
return;
|
||||
}
|
||||
|
||||
if (sigtype== DCCX_SIGNAL_FLAG) {
|
||||
case sigtypeDCCX:
|
||||
{
|
||||
// redpin,amberpin,greenpin are the 3 aspects
|
||||
byte value=redpin;
|
||||
if (rag==SIGNAL_AMBER) value=amberpin;
|
||||
if (rag==SIGNAL_GREEN) value=greenpin;
|
||||
DCC::setExtendedAccessory(sigid, value);
|
||||
auto value=signal.redpin;
|
||||
if (rag==SIGNAL_AMBER) value=signal.amberpin;
|
||||
if (rag==SIGNAL_GREEN) value=signal.greenpin;
|
||||
DCC::setExtendedAccessory(id, value);
|
||||
return;
|
||||
}
|
||||
|
||||
case sigtypeNEOPIXEL:
|
||||
{
|
||||
// redpin,amberpin,greenpin are the 3 RG values but with no blue permitted. . (code limitation hack)
|
||||
auto colour_RG=signal.redpin;
|
||||
if (rag==SIGNAL_AMBER) colour_RG=signal.amberpin;
|
||||
if (rag==SIGNAL_GREEN) colour_RG=signal.greenpin;
|
||||
|
||||
// blue channel is in followng signal slot (a continuation)
|
||||
auto signal2=getSignalSlot(sigslot+1);
|
||||
auto colour_B=signal2.redpin;
|
||||
if (rag==SIGNAL_AMBER) colour_B=signal2.amberpin;
|
||||
if (rag==SIGNAL_GREEN) colour_B=signal2.greenpin;
|
||||
IODevice::writeAnalogue(id, colour_RG,true,colour_B);
|
||||
return;
|
||||
}
|
||||
|
||||
case sigtypeSIGNAL:
|
||||
case sigtypeSIGNALH:
|
||||
{
|
||||
// LED or similar 3 pin signal, (all pins zero would be a virtual signal)
|
||||
// If amberpin is zero, synthesise amber from red+green
|
||||
const byte SIMAMBER=0x00;
|
||||
if (rag==SIGNAL_AMBER && (amberpin==0)) rag=SIMAMBER; // special case this func only
|
||||
if (rag==SIGNAL_AMBER && (signal.amberpin==0)) rag=SIMAMBER; // special case this func only
|
||||
|
||||
// Manage invert (HIGH on) pins
|
||||
bool aHigh=sighandle & ACTIVE_HIGH_SIGNAL_FLAG;
|
||||
bool aHigh=signal.type==sigtypeSIGNALH;
|
||||
|
||||
// set the three pins
|
||||
if (redpin) {
|
||||
if (signal.redpin) {
|
||||
bool redval=(rag==SIGNAL_RED || rag==SIMAMBER);
|
||||
if (!aHigh) redval=!redval;
|
||||
killBlinkOnVpin(redpin);
|
||||
IODevice::write(redpin,redval);
|
||||
killBlinkOnVpin(signal.redpin);
|
||||
IODevice::write(signal.redpin,redval);
|
||||
}
|
||||
if (amberpin) {
|
||||
if (signal.amberpin) {
|
||||
bool amberval=(rag==SIGNAL_AMBER);
|
||||
if (!aHigh) amberval=!amberval;
|
||||
killBlinkOnVpin(amberpin);
|
||||
IODevice::write(amberpin,amberval);
|
||||
killBlinkOnVpin(signal.amberpin);
|
||||
IODevice::write(signal.amberpin,amberval);
|
||||
}
|
||||
if (greenpin) {
|
||||
if (signal.greenpin) {
|
||||
bool greenval=(rag==SIGNAL_GREEN || rag==SIMAMBER);
|
||||
if (!aHigh) greenval=!greenval;
|
||||
killBlinkOnVpin(greenpin);
|
||||
IODevice::write(greenpin,greenval);
|
||||
killBlinkOnVpin(signal.greenpin);
|
||||
IODevice::write(signal.greenpin,greenval);
|
||||
}
|
||||
}
|
||||
case sigtypeVIRTUAL: break;
|
||||
case sigtypeContinuation: break;
|
||||
case sigtypeNoMoreSignals: break;
|
||||
}
|
||||
}
|
||||
|
||||
/* static */ bool RMFT2::isSignal(int16_t id,char rag) {
|
||||
if (!(compileFeatures & FEATURE_SIGNAL)) return false;
|
||||
int16_t sigslot=getSignalSlot(id);
|
||||
int16_t sigslot=signalLookup->find(id);
|
||||
if (sigslot<0) return false;
|
||||
return (flags[sigslot] & SIGNAL_MASK) == rag;
|
||||
}
|
||||
@@ -1255,26 +1266,23 @@ int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
// Otherwise false so the parser should send the command directly
|
||||
bool RMFT2::signalAspectEvent(int16_t address, byte aspect ) {
|
||||
if (!(compileFeatures & FEATURE_SIGNAL)) return false;
|
||||
int16_t sigslot=getSignalSlot(address);
|
||||
auto sigslot=signalLookup->find(address);
|
||||
if (sigslot<0) return false; // this is not a defined signal
|
||||
int16_t sigpos=sigslot*8;
|
||||
int16_t sighandle=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos);
|
||||
VPIN sigtype=sighandle & ~SIGNAL_ID_MASK;
|
||||
VPIN sigid = sighandle & SIGNAL_ID_MASK;
|
||||
if (sigtype!=DCCX_SIGNAL_FLAG) return false; // not a DCCX signal
|
||||
auto signal=getSignalSlot(sigslot);
|
||||
if (signal.type!=sigtypeDCCX) return false; // not a DCCX signal
|
||||
// Turn an aspect change into a RED/AMBER/GREEN setting
|
||||
if (aspect==GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+2)) {
|
||||
doSignal(sigid,SIGNAL_RED);
|
||||
if (aspect==signal.redpin) {
|
||||
doSignal(address,SIGNAL_RED);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (aspect==GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+4)) {
|
||||
doSignal(sigid,SIGNAL_AMBER);
|
||||
if (aspect==signal.amberpin) {
|
||||
doSignal(address,SIGNAL_AMBER);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (aspect==GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+6)) {
|
||||
doSignal(sigid,SIGNAL_GREEN);
|
||||
if (aspect==signal.greenpin) {
|
||||
doSignal(address,SIGNAL_GREEN);
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -1328,19 +1336,24 @@ void RMFT2::powerEvent(int16_t track, bool overload) {
|
||||
// This function is used when setting pins so that a SET or RESET
|
||||
// will cause any blink task on that pin to terminate.
|
||||
// It will be compiled out of existence if no BLINK feature is used.
|
||||
void RMFT2::killBlinkOnVpin(VPIN pin) {
|
||||
void RMFT2::killBlinkOnVpin(VPIN pin, uint16_t count) {
|
||||
if (!(compileFeatures & FEATURE_BLINK)) return;
|
||||
|
||||
RMFT2 * stoptask=loopTask; // stop when we get back to here
|
||||
RMFT2 * task=loopTask;
|
||||
VPIN lastPin=pin+count-1;
|
||||
while(task) {
|
||||
auto nextTask=task->next;
|
||||
if (
|
||||
(task->blinkState==blink_high || task->blinkState==blink_low)
|
||||
&& task->blinkPin==pin) {
|
||||
&& task->blinkPin>=pin
|
||||
&& task->blinkPin<=lastPin
|
||||
) {
|
||||
if (diag) DIAG(F("kill blink %d"),task->blinkPin,lastPin);
|
||||
task->kill();
|
||||
return;
|
||||
}
|
||||
task=task->next;
|
||||
if (task==loopTask) return;
|
||||
}
|
||||
task=nextTask;
|
||||
if (task==stoptask) return;
|
||||
}
|
||||
}
|
||||
|
||||
|
38
EXRAIL2.h
38
EXRAIL2.h
@@ -69,11 +69,14 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,OPCODE_TOGGLE_TURNOUT,
|
||||
OPCODE_TTADDPOSITION,OPCODE_DCCTURNTABLE,OPCODE_EXTTTURNTABLE,
|
||||
OPCODE_ONROTATE,OPCODE_ROTATE,OPCODE_WAITFORTT,
|
||||
OPCODE_LCC,OPCODE_LCCX,OPCODE_ONLCC,
|
||||
OPCODE_ACON, OPCODE_ACOF,
|
||||
OPCODE_ONACON, OPCODE_ONACOF,
|
||||
OPCODE_ONOVERLOAD,
|
||||
OPCODE_ROUTE_ACTIVE,OPCODE_ROUTE_INACTIVE,OPCODE_ROUTE_HIDDEN,
|
||||
OPCODE_ROUTE_DISABLED,
|
||||
OPCODE_STASH,OPCODE_CLEAR_STASH,OPCODE_CLEAR_ALL_STASH,OPCODE_PICKUP_STASH,
|
||||
OPCODE_ONBUTTON,OPCODE_ONSENSOR,
|
||||
OPCODE_ONBUTTON,OPCODE_ONSENSOR,
|
||||
OPCODE_NEOPIXEL,
|
||||
// OPcodes below this point are skip-nesting IF operations
|
||||
// placed here so that they may be skipped as a group
|
||||
// see skipIfBlock()
|
||||
@@ -107,6 +110,23 @@ enum BlinkState: byte {
|
||||
blink_high, // blink task running with pin high
|
||||
at_timeout // ATTIMEOUT timed out flag
|
||||
};
|
||||
enum SignalType {
|
||||
sigtypeVIRTUAL,
|
||||
sigtypeSIGNAL,
|
||||
sigtypeSIGNALH,
|
||||
sigtypeDCC,
|
||||
sigtypeDCCX,
|
||||
sigtypeSERVO,
|
||||
sigtypeNEOPIXEL,
|
||||
sigtypeContinuation, // neopixels require a second line
|
||||
sigtypeNoMoreSignals
|
||||
};
|
||||
|
||||
struct SIGNAL_DEFINITION {
|
||||
SignalType type;
|
||||
VPIN id;
|
||||
VPIN redpin,amberpin,greenpin;
|
||||
};
|
||||
|
||||
// Flag bits for compile time features.
|
||||
static const byte FEATURE_SIGNAL= 0x80;
|
||||
@@ -168,12 +188,7 @@ class LookList {
|
||||
static void rotateEvent(int16_t id, bool change);
|
||||
static void powerEvent(int16_t track, bool overload);
|
||||
static bool signalAspectEvent(int16_t address, byte aspect );
|
||||
static const int16_t SERVO_SIGNAL_FLAG=0x4000;
|
||||
static const int16_t ACTIVE_HIGH_SIGNAL_FLAG=0x2000;
|
||||
static const int16_t DCC_SIGNAL_FLAG=0x1000;
|
||||
static const int16_t DCCX_SIGNAL_FLAG=0x3000;
|
||||
static const int16_t SIGNAL_ID_MASK=0x0FFF;
|
||||
// Throttle Info Access functions built by exrail macros
|
||||
// Throttle Info Access functions built by exrail macros
|
||||
static const byte rosterNameCount;
|
||||
static const int16_t HIGHFLASH routeIdList[];
|
||||
static const int16_t HIGHFLASH automationIdList[];
|
||||
@@ -187,6 +202,8 @@ class LookList {
|
||||
static const FSH * getTurntablePositionDescription(int16_t turntableId, uint8_t positionId);
|
||||
static void startNonRecursiveTask(const FSH* reason, int16_t id,int pc);
|
||||
static bool readSensor(uint16_t sensorId);
|
||||
static bool isSignal(int16_t id,char rag);
|
||||
static SIGNAL_DEFINITION getSignalSlot(int16_t slotno);
|
||||
|
||||
private:
|
||||
static void ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16_t p[]);
|
||||
@@ -196,8 +213,6 @@ private:
|
||||
static bool getFlag(VPIN id,byte mask);
|
||||
static int16_t progtrackLocoId;
|
||||
static void doSignal(int16_t id,char rag);
|
||||
static bool isSignal(int16_t id,char rag);
|
||||
static int16_t getSignalSlot(int16_t id);
|
||||
static void setTurnoutHiddenState(Turnout * t);
|
||||
#ifndef IO_NO_HAL
|
||||
static void setTurntableHiddenState(Turntable * tto);
|
||||
@@ -205,7 +220,7 @@ private:
|
||||
static LookList* LookListLoader(OPCODE op1,
|
||||
OPCODE op2=OPCODE_ENDEXRAIL,OPCODE op3=OPCODE_ENDEXRAIL);
|
||||
static uint16_t getOperand(int progCounter,byte n);
|
||||
static void killBlinkOnVpin(VPIN pin);
|
||||
static void killBlinkOnVpin(VPIN pin,uint16_t count=1);
|
||||
static RMFT2 * loopTask;
|
||||
static RMFT2 * pausingTask;
|
||||
void delayMe(long millisecs);
|
||||
@@ -221,10 +236,11 @@ private:
|
||||
|
||||
static bool diag;
|
||||
static const HIGHFLASH3 byte RouteCode[];
|
||||
static const HIGHFLASH int16_t SignalDefinitions[];
|
||||
static const HIGHFLASH SIGNAL_DEFINITION SignalDefinitions[];
|
||||
static byte flags[MAX_FLAGS];
|
||||
static Print * LCCSerial;
|
||||
static LookList * routeLookup;
|
||||
static LookList * signalLookup;
|
||||
static LookList * onThrowLookup;
|
||||
static LookList * onCloseLookup;
|
||||
static LookList * onActivateLookup;
|
||||
|
@@ -99,6 +99,13 @@
|
||||
#undef LCCX
|
||||
#undef LCN
|
||||
#undef MOVETT
|
||||
#undef NEOPIXEL
|
||||
#undef NEOPIXEL_OFF
|
||||
#undef NEOPIXEL_SIGNAL
|
||||
#undef ACON
|
||||
#undef ACOF
|
||||
#undef ONACON
|
||||
#undef ONACOF
|
||||
#undef MESSAGE
|
||||
#undef ONACTIVATE
|
||||
#undef ONACTIVATEL
|
||||
@@ -191,7 +198,7 @@
|
||||
#ifndef RMFT2_UNDEF_ONLY
|
||||
#define ACTIVATE(addr,subaddr)
|
||||
#define ACTIVATEL(addr)
|
||||
#define AFTER(sensor_id)
|
||||
#define AFTER(sensor_id,timer...)
|
||||
#define AFTEROVERLOAD(track_id)
|
||||
#define ALIAS(name,value...)
|
||||
#define AMBER(signal_id)
|
||||
@@ -265,6 +272,12 @@
|
||||
#define LCN(msg)
|
||||
#define MESSAGE(msg)
|
||||
#define MOVETT(id,steps,activity)
|
||||
#define NEOPIXEL(id,r,g,b,count...)
|
||||
#define NEOPIXEL_SIGNAL(sigid,redcolour,ambercolour,greencolour)
|
||||
#define ACON(eventid)
|
||||
#define ACOF(eventid)
|
||||
#define ONACON(eventid)
|
||||
#define ONACOF(eventid)
|
||||
#define ONACTIVATE(addr,subaddr)
|
||||
#define ONACTIVATEL(linear)
|
||||
#define ONAMBER(signal_id)
|
||||
@@ -326,7 +339,7 @@
|
||||
#define SET_TRACK(track,mode)
|
||||
#define SET_POWER(track,onoff)
|
||||
#define SETLOCO(loco)
|
||||
#define SETFREQ(loco,freq)
|
||||
#define SETFREQ(freq)
|
||||
#define SIGNAL(redpin,amberpin,greenpin)
|
||||
#define SIGNALH(redpin,amberpin,greenpin)
|
||||
#define SPEED(speed)
|
||||
|
@@ -61,47 +61,85 @@ void RMFT2::ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16
|
||||
case 'L':
|
||||
// This entire code block is compiled out if LLC macros not used
|
||||
if (!(compileFeatures & FEATURE_LCC)) return;
|
||||
|
||||
static int lccProgCounter=0;
|
||||
static int lccEventIndex=0;
|
||||
|
||||
if (paramCount==0) { //<L> LCC adapter introducing self
|
||||
LCCSerial=stream; // now we know where to send events we raise
|
||||
opcode=0; // flag command as intercepted
|
||||
|
||||
// loop through all possible sent events
|
||||
for (int progCounter=0;; SKIPOP) {
|
||||
byte opcode=GET_OPCODE;
|
||||
if (opcode==OPCODE_ENDEXRAIL) break;
|
||||
if (opcode==OPCODE_LCC) StringFormatter::send(stream,F("<LS x%h>\n"),getOperand(progCounter,0));
|
||||
if (opcode==OPCODE_LCCX) { // long form LCC
|
||||
StringFormatter::send(stream,F("<LS x%h%h%h%h>\n"),
|
||||
// loop through all possible sent/waited events
|
||||
for (int progCounter=lccProgCounter;; SKIPOP) {
|
||||
byte exrailOpcode=GET_OPCODE;
|
||||
switch (exrailOpcode) {
|
||||
case OPCODE_ENDEXRAIL:
|
||||
stream->print(F("<LR>\n")); // ready to roll
|
||||
lccProgCounter=0; // allow a second pass
|
||||
lccEventIndex=0;
|
||||
return;
|
||||
|
||||
case OPCODE_LCC:
|
||||
StringFormatter::send(stream,F("<LS x%h>\n"),getOperand(progCounter,0));
|
||||
SKIPOP;
|
||||
lccProgCounter=progCounter;
|
||||
return;
|
||||
|
||||
case OPCODE_LCCX: // long form LCC
|
||||
StringFormatter::send(stream,F("<LS x%h%h%h%h>\n"),
|
||||
getOperand(progCounter,1),
|
||||
getOperand(progCounter,2),
|
||||
getOperand(progCounter,3),
|
||||
getOperand(progCounter,0)
|
||||
);
|
||||
}}
|
||||
);
|
||||
SKIPOP;SKIPOP;SKIPOP;SKIPOP;
|
||||
lccProgCounter=progCounter;
|
||||
return;
|
||||
|
||||
case OPCODE_ACON: // CBUS ACON
|
||||
case OPCODE_ACOF: // CBUS ACOF
|
||||
StringFormatter::send(stream,F("<LS x%c%h%h>\n"),
|
||||
exrailOpcode==OPCODE_ACOF?'1':'0',
|
||||
getOperand(progCounter,0),getOperand(progCounter,1));
|
||||
SKIPOP;SKIPOP;
|
||||
lccProgCounter=progCounter;
|
||||
return;
|
||||
|
||||
// we stream the hex events we wish to listen to
|
||||
// and at the same time build the event index looku.
|
||||
|
||||
|
||||
int eventIndex=0;
|
||||
for (int progCounter=0;; SKIPOP) {
|
||||
byte opcode=GET_OPCODE;
|
||||
if (opcode==OPCODE_ENDEXRAIL) break;
|
||||
if (opcode==OPCODE_ONLCC) {
|
||||
onLCCLookup[eventIndex]=progCounter; // TODO skip...
|
||||
case OPCODE_ONLCC:
|
||||
StringFormatter::send(stream,F("<LL %d x%h%h%h:%h>\n"),
|
||||
eventIndex,
|
||||
lccEventIndex,
|
||||
getOperand(progCounter,1),
|
||||
getOperand(progCounter,2),
|
||||
getOperand(progCounter,3),
|
||||
getOperand(progCounter,0)
|
||||
);
|
||||
eventIndex++;
|
||||
}
|
||||
SKIPOP;SKIPOP;SKIPOP;SKIPOP;
|
||||
// start on handler at next
|
||||
onLCCLookup[lccEventIndex]=progCounter;
|
||||
lccEventIndex++;
|
||||
lccProgCounter=progCounter;
|
||||
return;
|
||||
|
||||
case OPCODE_ONACON:
|
||||
case OPCODE_ONACOF:
|
||||
StringFormatter::send(stream,F("<LL %d x%c%h%h>\n"),
|
||||
lccEventIndex,
|
||||
exrailOpcode==OPCODE_ONACOF?'1':'0',
|
||||
getOperand(progCounter,0),getOperand(progCounter,1)
|
||||
);
|
||||
SKIPOP;SKIPOP;
|
||||
// start on handler at next
|
||||
onLCCLookup[lccEventIndex]=progCounter;
|
||||
lccEventIndex++;
|
||||
lccProgCounter=progCounter;
|
||||
return;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
StringFormatter::send(stream,F("<LR>\n")); // Ready to rumble
|
||||
opcode=0;
|
||||
break;
|
||||
}
|
||||
if (paramCount==1) { // <L eventid> LCC event arrived from adapter
|
||||
int16_t eventid=p[0];
|
||||
@@ -214,13 +252,13 @@ bool RMFT2::parseSlash(Print * stream, byte & paramCount, int16_t p[]) {
|
||||
// do the signals
|
||||
// flags[n] represents the state of the nth signal in the table
|
||||
for (int sigslot=0;;sigslot++) {
|
||||
int16_t sighandle=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigslot*8);
|
||||
if (sighandle==0) break; // end of signal list
|
||||
VPIN sigid = sighandle & SIGNAL_ID_MASK;
|
||||
byte flag=flags[sigslot] & SIGNAL_MASK; // obtain signal flags for this id
|
||||
SIGNAL_DEFINITION slot=getSignalSlot(sigslot);
|
||||
if (slot.type==sigtypeNoMoreSignals) break; // end of signal list
|
||||
if (slot.type==sigtypeContinuation) continue; // continueation of previous line
|
||||
byte flag=flags[sigslot] & SIGNAL_MASK; // obtain signal flags for this ids
|
||||
StringFormatter::send(stream,F("\n%S[%d]"),
|
||||
(flag == SIGNAL_RED)? F("RED") : (flag==SIGNAL_GREEN) ? F("GREEN") : F("AMBER"),
|
||||
sigid);
|
||||
slot.id);
|
||||
}
|
||||
}
|
||||
|
||||
|
@@ -63,6 +63,10 @@
|
||||
// playing sounds with IO_I2CDFPlayer
|
||||
#define PLAYSOUND ANOUT
|
||||
|
||||
// SEG7 is a helper to create ANOUT from a 7-segment request
|
||||
#define SEG7(vpin,value,format) \
|
||||
ANOUT(vpin,(value & 0xFFFF),TM1638::DF_##format,((uint32_t)value)>>16)
|
||||
|
||||
// helper macro to strip leading zeros off time inputs
|
||||
// (10#mins)%100)
|
||||
#define STRIP_ZERO(value) 10##value%100
|
||||
@@ -71,11 +75,13 @@
|
||||
//const byte TRACK_POWER_0=0, TRACK_POWER_OFF=0;
|
||||
//const byte TRACK_POWER_1=1, TRACK_POWER_ON=1;
|
||||
|
||||
// NEOPIXEL RG generator for NEOPIXEL_SIGNAL
|
||||
#define NeoRGB(red,green,blue) (((uint32_t)(red & 0xff)<<16) | ((uint32_t)(green & 0xff)<<8) | (uint32_t)(blue & 0xff))
|
||||
|
||||
// Pass 1 Implements aliases
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef ALIAS
|
||||
#define ALIAS(name,value...) const int name= 1##value##0 ==10 ? -__COUNTER__ : value##0/10;
|
||||
#define ALIAS(name,value...) const int name= #value[0] ? value+0: -__COUNTER__ ;
|
||||
#include "myAutomation.h"
|
||||
|
||||
// Pass 1d Detect sequence duplicates.
|
||||
@@ -180,6 +186,8 @@ bool exrailHalSetup() {
|
||||
#define DCC_SIGNAL(id,addr,subaddr) | FEATURE_SIGNAL
|
||||
#undef DCCX_SIGNAL
|
||||
#define DCCX_SIGNAL(id,redAspect,amberAspect,greenAspect) | FEATURE_SIGNAL
|
||||
#undef NEOPIXEL_SIGNAL
|
||||
#define NEOPIXEL_SIGNAL(sigid,redcolour,ambercolour,greencolour) | FEATURE_SIGNAL
|
||||
#undef VIRTUAL_SIGNAL
|
||||
#define VIRTUAL_SIGNAL(id) | FEATURE_SIGNAL
|
||||
|
||||
@@ -189,6 +197,14 @@ bool exrailHalSetup() {
|
||||
#define LCCX(senderid,eventid) | FEATURE_LCC
|
||||
#undef ONLCC
|
||||
#define ONLCC(senderid,eventid) | FEATURE_LCC
|
||||
#undef ACON
|
||||
#define ACON(eventid) | FEATURE_LCC
|
||||
#undef ACOF
|
||||
#define ACOF(eventid) | FEATURE_LCC
|
||||
#undef ONACON
|
||||
#define ONACON(eventid) | FEATURE_LCC
|
||||
#undef ONACOF
|
||||
#define ONACOF(eventid) | FEATURE_LCC
|
||||
#undef ROUTE_ACTIVE
|
||||
#define ROUTE_ACTIVE(id) | FEATURE_ROUTESTATE
|
||||
#undef ROUTE_INACTIVE
|
||||
@@ -413,26 +429,35 @@ const FSH * RMFT2::getRosterFunctions(int16_t id) {
|
||||
// Pass 8 Signal definitions
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef SIGNAL
|
||||
#define SIGNAL(redpin,amberpin,greenpin) redpin,redpin,amberpin,greenpin,
|
||||
#define SIGNAL(redpin,amberpin,greenpin) {sigtypeSIGNAL,redpin,redpin,amberpin,greenpin},
|
||||
#undef SIGNALH
|
||||
#define SIGNALH(redpin,amberpin,greenpin) redpin | RMFT2::ACTIVE_HIGH_SIGNAL_FLAG,redpin,amberpin,greenpin,
|
||||
#define SIGNALH(redpin,amberpin,greenpin) {sigtypeSIGNALH,redpin,redpin,amberpin,greenpin},
|
||||
#undef SERVO_SIGNAL
|
||||
#define SERVO_SIGNAL(vpin,redval,amberval,greenval) vpin | RMFT2::SERVO_SIGNAL_FLAG,redval,amberval,greenval,
|
||||
#define SERVO_SIGNAL(vpin,redval,amberval,greenval) {sigtypeSERVO,vpin,redval,amberval,greenval},
|
||||
#undef DCC_SIGNAL
|
||||
#define DCC_SIGNAL(id,addr,subaddr) id | RMFT2::DCC_SIGNAL_FLAG,addr,subaddr,0,
|
||||
#define DCC_SIGNAL(id,addr,subaddr) {sigtypeDCC,id,addr,subaddr,0},
|
||||
#undef DCCX_SIGNAL
|
||||
#define DCCX_SIGNAL(id,redAspect,amberAspect,greenAspect) id | RMFT2::DCCX_SIGNAL_FLAG,redAspect,amberAspect,greenAspect,
|
||||
#define DCCX_SIGNAL(id,redAspect,amberAspect,greenAspect) {sigtypeDCCX,id,redAspect,amberAspect,greenAspect},
|
||||
#undef NEOPIXEL_SIGNAL
|
||||
#define NEOPIXEL_SIGNAL(id,redRGB,amberRGB,greenRGB) \
|
||||
{sigtypeNEOPIXEL,id,((VPIN)((redRGB)>>8)), ((VPIN)((amberRGB)>>8)), ((VPIN)((greenRGB)>>8))},\
|
||||
{sigtypeContinuation,id,((VPIN)((redRGB) & 0xff)), ((VPIN)((amberRGB) & 0xFF)), ((VPIN)((greenRGB) & 0xFF))},
|
||||
#undef VIRTUAL_SIGNAL
|
||||
#define VIRTUAL_SIGNAL(id) id,0,0,0,
|
||||
#define VIRTUAL_SIGNAL(id) {sigtypeVIRTUAL,id,0,0,0},
|
||||
|
||||
const HIGHFLASH int16_t RMFT2::SignalDefinitions[] = {
|
||||
const HIGHFLASH SIGNAL_DEFINITION RMFT2::SignalDefinitions[] = {
|
||||
#include "myAutomation.h"
|
||||
0,0,0,0 };
|
||||
{sigtypeNoMoreSignals,0,0,0,0}
|
||||
};
|
||||
|
||||
// Pass 9 ONLCC counter and lookup array
|
||||
// Pass 9 ONLCC/ ONMERG counter and lookup array
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef ONLCC
|
||||
#define ONLCC(sender,event) +1
|
||||
#undef ONACON
|
||||
#define ONACON(event) +1
|
||||
#undef ONACOF
|
||||
#define ONACOF(event) +1
|
||||
|
||||
const int RMFT2::countLCCLookup=0
|
||||
#include "myAutomation.h"
|
||||
@@ -451,7 +476,7 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
|
||||
#define ACTIVATE(addr,subaddr) OPCODE_DCCACTIVATE,V(addr<<3 | subaddr<<1 | 1),
|
||||
#define ACTIVATEL(addr) OPCODE_DCCACTIVATE,V((addr+3)<<1 | 1),
|
||||
#define AFTER(sensor_id) OPCODE_AT,V(sensor_id),OPCODE_AFTER,V(sensor_id),
|
||||
#define AFTER(sensor_id,timer...) OPCODE_AT,V(sensor_id),OPCODE_AFTER,V(sensor_id),OPCODE_PAD,V(#timer[0]?timer+0:500),
|
||||
#define AFTEROVERLOAD(track_id) OPCODE_AFTEROVERLOAD,V(TRACK_NUMBER_##track_id),
|
||||
#define ALIAS(name,value...)
|
||||
#define AMBER(signal_id) OPCODE_AMBER,V(signal_id),
|
||||
@@ -529,6 +554,10 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
OPCODE_PAD,V((((uint64_t)sender)>>32)&0xFFFF),\
|
||||
OPCODE_PAD,V((((uint64_t)sender)>>16)&0xFFFF),\
|
||||
OPCODE_PAD,V((((uint64_t)sender)>>0)&0xFFFF),
|
||||
#define ACON(eventid) OPCODE_ACON,V(((uint32_t)eventid >>16) & 0xFFFF),OPCODE_PAD,V(eventid & 0xFFFF),
|
||||
#define ACOF(eventid) OPCODE_ACOF,V(((uint32_t)eventid >>16) & 0xFFFF),OPCODE_PAD,V(eventid & 0xFFFF),
|
||||
#define ONACON(eventid) OPCODE_ONACON,V((uint32_t)(eventid) >>16),OPCODE_PAD,V(eventid & 0xFFFF),
|
||||
#define ONACOF(eventid) OPCODE_ONACOF,V((uint32_t)(eventid) >>16),OPCODE_PAD,V(eventid & 0xFFFF),
|
||||
#define LCD(id,msg) PRINT(msg)
|
||||
#define SCREEN(display,id,msg) PRINT(msg)
|
||||
#define STEALTH(code...) PRINT(dummy)
|
||||
@@ -536,6 +565,12 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define LCN(msg) PRINT(msg)
|
||||
#define MESSAGE(msg) PRINT(msg)
|
||||
#define MOVETT(id,steps,activity) OPCODE_SERVO,V(id),OPCODE_PAD,V(steps),OPCODE_PAD,V(EXTurntable::activity),OPCODE_PAD,V(0),
|
||||
#define NEOPIXEL(id,r,g,b,count...) OPCODE_NEOPIXEL,V(id),\
|
||||
OPCODE_PAD,V(((r & 0xff)<<8) | (g & 0xff)),\
|
||||
OPCODE_PAD,V((b & 0xff)),\
|
||||
OPCODE_PAD,V(#count[0]?(count+0):1),
|
||||
|
||||
#define NEOPIXEL_SIGNAL(sigid,redcolour,ambercolour,greencolour)
|
||||
#define ONACTIVATE(addr,subaddr) OPCODE_ONACTIVATE,V(addr<<2|subaddr),
|
||||
#define ONACTIVATEL(linear) OPCODE_ONACTIVATE,V(linear+3),
|
||||
#define ONAMBER(signal_id) OPCODE_ONAMBER,V(signal_id),
|
||||
@@ -604,7 +639,7 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define SET_TRACK(track,mode) OPCODE_SET_TRACK,V(TRACK_MODE_##mode <<8 | TRACK_NUMBER_##track),
|
||||
#define SET_POWER(track,onoff) OPCODE_SET_POWER,V(TRACK_POWER_##onoff),OPCODE_PAD, V(TRACK_NUMBER_##track),
|
||||
#define SETLOCO(loco) OPCODE_SETLOCO,V(loco),
|
||||
#define SETFREQ(loco,freq) OPCODE_SETLOCO,V(loco), OPCODE_SETFREQ,V(freq),
|
||||
#define SETFREQ(freq) OPCODE_SETFREQ,V(freq),
|
||||
#define SIGNAL(redpin,amberpin,greenpin)
|
||||
#define SIGNALH(redpin,amberpin,greenpin)
|
||||
#define SPEED(speed) OPCODE_SPEED,V(speed),
|
||||
|
@@ -1,8 +1,10 @@
|
||||
/*
|
||||
* © 2024 Morten "Doc" Nielsen
|
||||
* © 2023-2024 Paul M. Antoine
|
||||
* © 2022 Bruno Sanches
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2022 Harald Barth
|
||||
* © 2020-2021 Chris Harlow
|
||||
* © 2020-2024 Chris Harlow
|
||||
* © 2020 Gregor Baues
|
||||
* All rights reserved.
|
||||
*
|
||||
@@ -29,76 +31,139 @@
|
||||
#include "CommandDistributor.h"
|
||||
#include "WiThrottle.h"
|
||||
#include "DCCTimer.h"
|
||||
#if __has_include ( "MDNS_Generic.h")
|
||||
#include "MDNS_Generic.h"
|
||||
#define DO_MDNS
|
||||
EthernetUDP udp;
|
||||
MDNS mdns(udp);
|
||||
#endif
|
||||
|
||||
|
||||
//extern void looptimer(unsigned long timeout, const FSH* message);
|
||||
#define looptimer(a,b)
|
||||
|
||||
bool EthernetInterface::connected=false;
|
||||
EthernetServer * EthernetInterface::server= nullptr;
|
||||
EthernetClient EthernetInterface::clients[MAX_SOCK_NUM]; // accept up to MAX_SOCK_NUM client connections at the same time; This depends on the chipset used on the Shield
|
||||
bool EthernetInterface::inUse[MAX_SOCK_NUM]; // accept up to MAX_SOCK_NUM client connections at the same time; This depends on the chipset used on the Shield
|
||||
uint8_t EthernetInterface::buffer[MAX_ETH_BUFFER+1]; // buffer used by TCP for the recv
|
||||
RingStream * EthernetInterface::outboundRing = nullptr;
|
||||
|
||||
EthernetInterface * EthernetInterface::singleton=NULL;
|
||||
/**
|
||||
* @brief Setup Ethernet Connection
|
||||
*
|
||||
*/
|
||||
void EthernetInterface::setup()
|
||||
|
||||
void EthernetInterface::setup()
|
||||
{
|
||||
if (singleton!=NULL) {
|
||||
DIAG(F("Prog Error!"));
|
||||
return;
|
||||
}
|
||||
if ((singleton=new EthernetInterface()))
|
||||
return;
|
||||
DIAG(F("Ethernet not initialized"));
|
||||
};
|
||||
DIAG(F("Ethernet starting"
|
||||
#ifdef DO_MDNS
|
||||
" (with mDNS)"
|
||||
#endif
|
||||
" Please be patient, especially if no cable is connected!"
|
||||
));
|
||||
|
||||
#ifdef STM32_ETHERNET
|
||||
// Set a HOSTNAME for the DHCP request - a nice to have, but hard it seems on LWIP for STM32
|
||||
// The default is "lwip", which is **always** set in STM32Ethernet/src/utility/ethernetif.cpp
|
||||
// for some reason. One can edit it to instead read:
|
||||
// #if LWIP_NETIF_HOSTNAME
|
||||
// /* Initialize interface hostname */
|
||||
// if (netif->hostname == NULL)
|
||||
// netif->hostname = "lwip";
|
||||
// #endif /* LWIP_NETIF_HOSTNAME */
|
||||
// Which seems more useful! We should propose the patch... so the following line actually works!
|
||||
netif_set_hostname(&gnetif, WIFI_HOSTNAME); // Should probably be passed in the contructor...
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef IP_ADDRESS
|
||||
static IPAddress myIP(IP_ADDRESS);
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Aquire IP Address from DHCP and start server
|
||||
*
|
||||
* @return true
|
||||
* @return false
|
||||
*/
|
||||
EthernetInterface::EthernetInterface()
|
||||
{
|
||||
byte mac[6];
|
||||
DCCTimer::getSimulatedMacAddress(mac);
|
||||
connected=false;
|
||||
|
||||
#ifdef IP_ADDRESS
|
||||
Ethernet.begin(mac, myIP);
|
||||
#else
|
||||
if (Ethernet.begin(mac) == 0)
|
||||
{
|
||||
DIAG(F("Ethernet.begin FAILED"));
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
if (Ethernet.hardwareStatus() == EthernetNoHardware) {
|
||||
DIAG(F("Ethernet shield not found or W5100"));
|
||||
}
|
||||
|
||||
unsigned long startmilli = millis();
|
||||
while ((millis() - startmilli) < 5500) { // Loop to give time to check for cable connection
|
||||
if (Ethernet.linkStatus() == LinkON)
|
||||
break;
|
||||
DIAG(F("Ethernet waiting for link (1sec) "));
|
||||
delay(1000);
|
||||
}
|
||||
// now we either do have link of we have a W5100
|
||||
// where we do not know if we have link. That's
|
||||
// the reason to now run checkLink.
|
||||
// CheckLinks sets up outboundRing if it does
|
||||
// not exist yet as well.
|
||||
checkLink();
|
||||
#ifdef IP_ADDRESS
|
||||
static IPAddress myIP(IP_ADDRESS);
|
||||
Ethernet.begin(mac,myIP);
|
||||
#else
|
||||
if (Ethernet.begin(mac)==0)
|
||||
{
|
||||
LCD(4,F("IP: No DHCP"));
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
auto ip = Ethernet.localIP(); // look what IP was obtained (dynamic or static)
|
||||
if (!ip) {
|
||||
LCD(4,F("IP: None"));
|
||||
return;
|
||||
}
|
||||
server = new EthernetServer(IP_PORT); // Ethernet Server listening on default port IP_PORT
|
||||
server->begin();
|
||||
|
||||
// Arrange display of IP address and port
|
||||
#ifdef LCD_DRIVER
|
||||
const byte lcdData[]={LCD_DRIVER};
|
||||
const bool wideDisplay=lcdData[1]>=24; // data[1] is cols.
|
||||
#else
|
||||
const bool wideDisplay=true;
|
||||
#endif
|
||||
if (wideDisplay) {
|
||||
// OLEDS or just usb diag is ok on one line.
|
||||
LCD(4,F("IP %d.%d.%d.%d:%d"), ip[0], ip[1], ip[2], ip[3], IP_PORT);
|
||||
}
|
||||
else { // LCDs generally too narrow, so take 2 lines
|
||||
LCD(4,F("IP %d.%d.%d.%d"), ip[0], ip[1], ip[2], ip[3]);
|
||||
LCD(5,F("Port %d"), IP_PORT);
|
||||
}
|
||||
|
||||
outboundRing=new RingStream(OUTBOUND_RING_SIZE);
|
||||
#ifdef DO_MDNS
|
||||
mdns.begin(Ethernet.localIP(), WIFI_HOSTNAME); // hostname
|
||||
mdns.addServiceRecord(WIFI_HOSTNAME "._withrottle", IP_PORT, MDNSServiceTCP);
|
||||
// Not sure if we need to run it once, but just in case!
|
||||
mdns.run();
|
||||
#endif
|
||||
connected=true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cleanup any resources
|
||||
*
|
||||
* @return none
|
||||
*/
|
||||
EthernetInterface::~EthernetInterface() {
|
||||
delete server;
|
||||
delete outboundRing;
|
||||
#if defined (STM32_ETHERNET)
|
||||
void EthernetInterface::acceptClient() { // STM32 version
|
||||
auto client=server->available();
|
||||
if (!client) return;
|
||||
// check for existing client
|
||||
for (byte socket = 0; socket < MAX_SOCK_NUM; socket++)
|
||||
if (inUse[socket] && client == clients[socket]) return;
|
||||
|
||||
// new client
|
||||
for (byte socket = 0; socket < MAX_SOCK_NUM; socket++)
|
||||
{
|
||||
if (!inUse[socket])
|
||||
{
|
||||
clients[socket] = client;
|
||||
inUse[socket]=true;
|
||||
if (Diag::ETHERNET)
|
||||
DIAG(F("Ethernet: New client socket %d"), socket);
|
||||
return;
|
||||
}
|
||||
}
|
||||
DIAG(F("Ethernet OVERFLOW"));
|
||||
}
|
||||
#else
|
||||
void EthernetInterface::acceptClient() { // non-STM32 version
|
||||
auto client=server->accept();
|
||||
if (!client) return;
|
||||
auto socket=client.getSocketNumber();
|
||||
clients[socket]=client;
|
||||
inUse[socket]=true;
|
||||
if (Diag::ETHERNET)
|
||||
DIAG(F("Ethernet: New client socket %d"), socket);
|
||||
}
|
||||
#endif
|
||||
|
||||
void EthernetInterface::dropClient(byte socket)
|
||||
{
|
||||
clients[socket].stop();
|
||||
inUse[socket]=false;
|
||||
CommandDistributor::forget(socket);
|
||||
if (Diag::ETHERNET) DIAG(F("Ethernet: Disconnect %d "), socket);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -107,134 +172,109 @@ EthernetInterface::~EthernetInterface() {
|
||||
*/
|
||||
void EthernetInterface::loop()
|
||||
{
|
||||
if (!singleton || (!singleton->checkLink()))
|
||||
return;
|
||||
if (!connected) return;
|
||||
looptimer(5000, F("E.loop"));
|
||||
|
||||
static bool warnedAboutLink=false;
|
||||
if (Ethernet.linkStatus() == LinkOFF){
|
||||
if (warnedAboutLink) return;
|
||||
DIAG(F("Ethernet link OFF"));
|
||||
warnedAboutLink=true;
|
||||
return;
|
||||
}
|
||||
looptimer(5000, F("E.loop warn"));
|
||||
|
||||
// link status must be ok here
|
||||
if (warnedAboutLink) {
|
||||
DIAG(F("Ethernet link RESTORED"));
|
||||
warnedAboutLink=false;
|
||||
}
|
||||
|
||||
#ifdef DO_MDNS
|
||||
// Always do this because we don't want traffic to intefere with being found!
|
||||
mdns.run();
|
||||
looptimer(5000, F("E.mdns"));
|
||||
|
||||
#endif
|
||||
|
||||
//
|
||||
switch (Ethernet.maintain()) {
|
||||
case 1:
|
||||
//renewed fail
|
||||
DIAG(F("Ethernet Error: renewed fail"));
|
||||
singleton=NULL;
|
||||
connected=false;
|
||||
return;
|
||||
case 3:
|
||||
//rebind fail
|
||||
DIAG(F("Ethernet Error: rebind fail"));
|
||||
singleton=NULL;
|
||||
connected=false;
|
||||
return;
|
||||
default:
|
||||
//nothing happened
|
||||
//DIAG(F("maintained"));
|
||||
break;
|
||||
}
|
||||
singleton->loop2();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks ethernet link cable status and detects when it connects / disconnects
|
||||
*
|
||||
* @return true when cable is connected, false otherwise
|
||||
*/
|
||||
bool EthernetInterface::checkLink() {
|
||||
if (Ethernet.linkStatus() != LinkOFF) { // check for not linkOFF instead of linkON as the W5100 does return LinkUnknown
|
||||
//if we are not connected yet, setup a new server
|
||||
if(!connected) {
|
||||
DIAG(F("Ethernet cable connected"));
|
||||
connected=true;
|
||||
#ifdef IP_ADDRESS
|
||||
Ethernet.setLocalIP(myIP); // for static IP, set it again
|
||||
#endif
|
||||
IPAddress ip = Ethernet.localIP(); // look what IP was obtained (dynamic or static)
|
||||
server = new EthernetServer(IP_PORT); // Ethernet Server listening on default port IP_PORT
|
||||
server->begin();
|
||||
LCD(4,F("IP: %d.%d.%d.%d"), ip[0], ip[1], ip[2], ip[3]);
|
||||
LCD(5,F("Port:%d"), IP_PORT);
|
||||
// only create a outboundRing it none exists, this may happen if the cable
|
||||
// gets disconnected and connected again
|
||||
if(!outboundRing)
|
||||
outboundRing=new RingStream(OUTBOUND_RING_SIZE);
|
||||
}
|
||||
return true;
|
||||
} else { // connected
|
||||
DIAG(F("Ethernet cable disconnected"));
|
||||
connected=false;
|
||||
//clean up any client
|
||||
for (byte socket = 0; socket < MAX_SOCK_NUM; socket++) {
|
||||
if(clients[socket].connected())
|
||||
clients[socket].stop();
|
||||
}
|
||||
// tear down server
|
||||
delete server;
|
||||
server = nullptr;
|
||||
LCD(4,F("IP: None"));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void EthernetInterface::loop2() {
|
||||
if (!outboundRing) { // no idea to call loop2() if we can't handle outgoing data in it
|
||||
if (Diag::ETHERNET) DIAG(F("No outboundRing"));
|
||||
return;
|
||||
}
|
||||
looptimer(5000, F("E.maintain"));
|
||||
|
||||
// get client from the server
|
||||
EthernetClient client = server->accept();
|
||||
|
||||
// check for new client
|
||||
if (client)
|
||||
acceptClient();
|
||||
|
||||
// handle disconnected sockets because STM32 library doesnt
|
||||
// do the read==0 response.
|
||||
for (byte socket = 0; socket < MAX_SOCK_NUM; socket++)
|
||||
{
|
||||
if (Diag::ETHERNET) DIAG(F("Ethernet: New client "));
|
||||
byte socket;
|
||||
for (socket = 0; socket < MAX_SOCK_NUM; socket++)
|
||||
{
|
||||
if (!clients[socket])
|
||||
{
|
||||
// On accept() the EthernetServer doesn't track the client anymore
|
||||
// so we store it in our client array
|
||||
if (Diag::ETHERNET) DIAG(F("Socket %d"),socket);
|
||||
clients[socket] = client;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (socket==MAX_SOCK_NUM) DIAG(F("new Ethernet OVERFLOW"));
|
||||
}
|
||||
if (inUse[socket] && !clients[socket].connected()) dropClient(socket);
|
||||
}
|
||||
|
||||
// check for incoming data from all possible clients
|
||||
for (byte socket = 0; socket < MAX_SOCK_NUM; socket++)
|
||||
{
|
||||
if (clients[socket]) {
|
||||
|
||||
int available=clients[socket].available();
|
||||
if (available > 0) {
|
||||
if (Diag::ETHERNET) DIAG(F("Ethernet: available socket=%d,avail=%d"), socket, available);
|
||||
// read bytes from a client
|
||||
int count = clients[socket].read(buffer, MAX_ETH_BUFFER);
|
||||
buffer[count] = '\0'; // terminate the string properly
|
||||
if (Diag::ETHERNET) DIAG(F(",count=%d:%e"), socket,buffer);
|
||||
// execute with data going directly back
|
||||
CommandDistributor::parse(socket,buffer,outboundRing);
|
||||
return; // limit the amount of processing that takes place within 1 loop() cycle.
|
||||
}
|
||||
}
|
||||
if (!inUse[socket]) continue; // socket is not in use
|
||||
|
||||
// read any bytes from this client
|
||||
auto count = clients[socket].read(buffer, MAX_ETH_BUFFER);
|
||||
|
||||
if (count<0) continue; // -1 indicates nothing to read
|
||||
|
||||
if (count > 0) { // we have incoming data
|
||||
buffer[count] = '\0'; // terminate the string properly
|
||||
if (Diag::ETHERNET) DIAG(F("Ethernet s=%d, c=%d b=:%e"), socket, count, buffer);
|
||||
// execute with data going directly back
|
||||
CommandDistributor::parse(socket,buffer,outboundRing);
|
||||
//looptimer(5000, F("Ethloop2 parse"));
|
||||
return; // limit the amount of processing that takes place within 1 loop() cycle.
|
||||
}
|
||||
|
||||
// count=0 The client has disconnected
|
||||
dropClient(socket);
|
||||
}
|
||||
|
||||
// stop any clients which disconnect
|
||||
for (int socket = 0; socket<MAX_SOCK_NUM; socket++) {
|
||||
if (clients[socket] && !clients[socket].connected()) {
|
||||
clients[socket].stop();
|
||||
CommandDistributor::forget(socket);
|
||||
if (Diag::ETHERNET) DIAG(F("Ethernet: disconnect %d "), socket);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
WiThrottle::loop(outboundRing);
|
||||
|
||||
|
||||
// handle at most 1 outbound transmission
|
||||
int socketOut=outboundRing->read();
|
||||
auto socketOut=outboundRing->read();
|
||||
if (socketOut<0) return; // no outbound pending
|
||||
|
||||
if (socketOut >= MAX_SOCK_NUM) {
|
||||
DIAG(F("Ethernet outboundRing socket=%d error"), socketOut);
|
||||
} else if (socketOut >= 0) {
|
||||
int count=outboundRing->count();
|
||||
if (Diag::ETHERNET) DIAG(F("Ethernet reply socket=%d, count=:%d"), socketOut,count);
|
||||
for(;count>0;count--) clients[socketOut].write(outboundRing->read());
|
||||
clients[socketOut].flush(); //maybe
|
||||
// This is a catastrophic code failure and unrecoverable.
|
||||
DIAG(F("Ethernet outboundRing s=%d error"), socketOut);
|
||||
connected=false;
|
||||
return;
|
||||
}
|
||||
|
||||
auto count=outboundRing->count();
|
||||
{
|
||||
char tmpbuf[count+1]; // one extra for '\0'
|
||||
for(int i=0;i<count;i++) {
|
||||
tmpbuf[i] = outboundRing->read();
|
||||
}
|
||||
tmpbuf[count]=0;
|
||||
if (inUse[socketOut]) {
|
||||
if (Diag::ETHERNET) DIAG(F("Ethernet reply s=%d, c=%d, b:%e"),
|
||||
socketOut,count,tmpbuf);
|
||||
clients[socketOut].write(tmpbuf,count);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
|
@@ -1,8 +1,10 @@
|
||||
/*
|
||||
* © 2023-2024 Paul M. Antoine
|
||||
* © 2021 Neil McKechnie
|
||||
* © 2021 Mike S
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2021 Chris Harlow
|
||||
* © 2020-2022 Harald Barth
|
||||
* © 2020-2024 Chris Harlow
|
||||
* © 2020 Gregor Baues
|
||||
* All rights reserved.
|
||||
*
|
||||
@@ -35,6 +37,15 @@
|
||||
#if defined (ARDUINO_TEENSY41)
|
||||
#include <NativeEthernet.h> //TEENSY Ethernet Treiber
|
||||
#include <NativeEthernetUdp.h>
|
||||
#define MAX_SOCK_NUM 4
|
||||
#elif defined (ARDUINO_NUCLEO_F429ZI) || defined (ARDUINO_NUCLEO_F439ZI) || defined (ARDUINO_NUCLEO_F4X9ZI)
|
||||
#include <LwIP.h>
|
||||
// #include "STM32lwipopts.h"
|
||||
#include <STM32Ethernet.h>
|
||||
#include <lwip/netif.h>
|
||||
extern "C" struct netif gnetif;
|
||||
#define STM32_ETHERNET
|
||||
#define MAX_SOCK_NUM 8
|
||||
#else
|
||||
#include "Ethernet.h"
|
||||
#endif
|
||||
@@ -45,7 +56,7 @@
|
||||
*
|
||||
*/
|
||||
|
||||
#define MAX_ETH_BUFFER 512
|
||||
#define MAX_ETH_BUFFER 128
|
||||
#define OUTBOUND_RING_SIZE 2048
|
||||
|
||||
class EthernetInterface {
|
||||
@@ -56,16 +67,15 @@ class EthernetInterface {
|
||||
static void loop();
|
||||
|
||||
private:
|
||||
static EthernetInterface * singleton;
|
||||
bool connected;
|
||||
EthernetInterface();
|
||||
~EthernetInterface();
|
||||
void loop2();
|
||||
bool checkLink();
|
||||
EthernetServer * server = NULL;
|
||||
EthernetClient clients[MAX_SOCK_NUM]; // accept up to MAX_SOCK_NUM client connections at the same time; This depends on the chipset used on the Shield
|
||||
uint8_t buffer[MAX_ETH_BUFFER+1]; // buffer used by TCP for the recv
|
||||
RingStream * outboundRing = NULL;
|
||||
static bool connected;
|
||||
static EthernetServer * server;
|
||||
static EthernetClient clients[MAX_SOCK_NUM]; // accept up to MAX_SOCK_NUM client connections at the same time; This depends on the chipset used on the Shield
|
||||
static bool inUse[MAX_SOCK_NUM]; // accept up to MAX_SOCK_NUM client connections at the same time; This depends on the chipset used on the Shield
|
||||
static uint8_t buffer[MAX_ETH_BUFFER+1]; // buffer used by TCP for the recv
|
||||
static RingStream * outboundRing;
|
||||
static void acceptClient();
|
||||
static void dropClient(byte socketnum);
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
8
FSH.h
8
FSH.h
@@ -52,6 +52,7 @@ typedef __FlashStringHelper FSH;
|
||||
#define STRNCPY_P strncpy_P
|
||||
#define STRNCMP_P strncmp_P
|
||||
#define STRLEN_P strlen_P
|
||||
#define STRCHR_P strchr_P
|
||||
|
||||
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
|
||||
// AVR_MEGA memory deliberately placed at end of link may need _far functions
|
||||
@@ -60,6 +61,8 @@ typedef __FlashStringHelper FSH;
|
||||
#define GETFARPTR(data) pgm_get_far_address(data)
|
||||
#define GETHIGHFLASH(data,offset) pgm_read_byte_far(GETFARPTR(data)+offset)
|
||||
#define GETHIGHFLASHW(data,offset) pgm_read_word_far(GETFARPTR(data)+offset)
|
||||
#define COPYHIGHFLASH(target,base,offset,length) \
|
||||
memcpy_PF(target,GETFARPTR(base) + offset,length)
|
||||
#else
|
||||
// AVR_UNO/NANO runtime does not support _far functions so just use _near equivalent
|
||||
// as there is no progmem above 32kb anyway.
|
||||
@@ -68,6 +71,8 @@ typedef __FlashStringHelper FSH;
|
||||
#define GETFARPTR(data) ((uint32_t)(data))
|
||||
#define GETHIGHFLASH(data,offset) pgm_read_byte_near(GETFARPTR(data)+(offset))
|
||||
#define GETHIGHFLASHW(data,offset) pgm_read_word_near(GETFARPTR(data)+(offset))
|
||||
#define COPYHIGHFLASH(target,base,offset,length) \
|
||||
memcpy_P(target,(byte *)base + offset,length)
|
||||
#endif
|
||||
|
||||
#else
|
||||
@@ -87,10 +92,13 @@ typedef char FSH;
|
||||
#define GETFLASH(addr) (*(const byte *)(addr))
|
||||
#define GETHIGHFLASH(data,offset) (*(const byte *)(GETFARPTR(data)+offset))
|
||||
#define GETHIGHFLASHW(data,offset) (*(const uint16_t *)(GETFARPTR(data)+offset))
|
||||
#define COPYHIGHFLASH(target,base,offset,length) \
|
||||
memcpy(target,(byte *)&base + offset,length)
|
||||
#define STRCPY_P strcpy
|
||||
#define STRCMP_P strcmp
|
||||
#define STRNCPY_P strncpy
|
||||
#define STRNCMP_P strncmp
|
||||
#define STRLEN_P strlen
|
||||
#define STRCHR_P strchr
|
||||
#endif
|
||||
#endif
|
||||
|
@@ -1 +1 @@
|
||||
#define GITHUB_SHA "devel-202404211704Z"
|
||||
#define GITHUB_SHA "devel-heartbeat202410192325ZZ"
|
||||
|
@@ -46,27 +46,37 @@
|
||||
|
||||
// Helper function for listing device types
|
||||
static const FSH * guessI2CDeviceType(uint8_t address) {
|
||||
if (address == 0x1A)
|
||||
// 0x09-0x18 selectable, but for now handle the default
|
||||
return F("Piicodev 865/915MHz Transceiver");
|
||||
if (address == 0x1C)
|
||||
return F("QMC6310 Magnetometer");
|
||||
if (address >= 0x20 && address <= 0x26)
|
||||
return F("GPIO Expander");
|
||||
else if (address == 0x27)
|
||||
if (address == 0x27)
|
||||
return F("GPIO Expander or LCD Display");
|
||||
else if (address == 0x29)
|
||||
if (address == 0x29)
|
||||
return F("Time-of-flight sensor");
|
||||
else if (address >= 0x3c && address <= 0x3d)
|
||||
return F("OLED Display");
|
||||
else if (address >= 0x48 && address <= 0x57) // SC16IS752x UART detection
|
||||
if (address == 0x34)
|
||||
return F("TCA8418 keypad scanner");
|
||||
if (address >= 0x3c && address <= 0x3d)
|
||||
// 0x3c can also be an HMC883L magnetometer
|
||||
return F("OLED Display or HMC583L Magnetometer");
|
||||
if (address >= 0x48 && address <= 0x57) // SC16IS752x UART detection
|
||||
return F("SC16IS75x UART");
|
||||
else if (address >= 0x48 && address <= 0x4f)
|
||||
if (address >= 0x48 && address <= 0x4f)
|
||||
return F("Analogue Inputs or PWM");
|
||||
else if (address >= 0x40 && address <= 0x4f)
|
||||
if (address >= 0x40 && address <= 0x4f)
|
||||
return F("PWM");
|
||||
else if (address >= 0x50 && address <= 0x5f)
|
||||
if (address >= 0x50 && address <= 0x5f)
|
||||
return F("EEPROM");
|
||||
else if (address == 0x68)
|
||||
if (address >= 0x60 && address <= 0x68)
|
||||
return F("Adafruit NeoPixel Driver");
|
||||
if (address == 0x68)
|
||||
return F("Real-time clock");
|
||||
else if (address >= 0x70 && address <= 0x77)
|
||||
if (address >= 0x70 && address <= 0x77)
|
||||
return F("I2C Mux");
|
||||
else
|
||||
// Unknown type
|
||||
return F("?");
|
||||
}
|
||||
|
||||
|
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
* © 2022-23 Paul M Antoine
|
||||
* © 2022-24 Paul M Antoine
|
||||
* © 2023, Neil McKechnie
|
||||
* All rights reserved.
|
||||
*
|
||||
@@ -38,8 +38,9 @@
|
||||
*****************************************************************************/
|
||||
#if defined(I2C_USE_INTERRUPTS) && defined(ARDUINO_ARCH_STM32)
|
||||
#if defined(ARDUINO_NUCLEO_F401RE) || defined(ARDUINO_NUCLEO_F411RE) || defined(ARDUINO_NUCLEO_F446RE) \
|
||||
|| defined(ARDUINO_NUCLEO_F412ZG) || defined(ARDUINO_NUCLEO_F413ZH) \
|
||||
|| defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F446ZE)
|
||||
|| defined(ARDUINO_NUCLEO_F412ZG) || defined(ARDUINO_NUCLEO_F413ZH) || defined(ARDUINO_NUCLEO_F446ZE) \
|
||||
|| defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F439ZI) || defined(ARDUINO_NUCLEO_F4X9ZI)
|
||||
|
||||
// Assume I2C1 for now - default I2C bus on Nucleo-F411RE and likely all Nucleo-64
|
||||
// and Nucleo-144 variants
|
||||
I2C_TypeDef *s = I2C1;
|
||||
@@ -184,7 +185,7 @@ void I2CManagerClass::I2C_init()
|
||||
GPIOB->OTYPER |= (1<<8) | (1<<9); // PB8 and PB9 set to open drain output capability
|
||||
GPIOB->OSPEEDR |= (3<<(8*2)) | (3<<(9*2)); // PB8 and PB9 set to High Speed mode
|
||||
GPIOB->PUPDR &= ~((3<<(8*2)) | (3<<(9*2))); // Clear all PUPDR bits for PB8 and PB9
|
||||
GPIOB->PUPDR |= (1<<(8*2)) | (1<<(9*2)); // PB8 and PB9 set to pull-up capability
|
||||
// GPIOB->PUPDR |= (1<<(8*2)) | (1<<(9*2)); // PB8 and PB9 set to pull-up capability
|
||||
// Alt Function High register routing pins PB8 and PB9 for I2C1:
|
||||
// Bits (3:2:1:0) = 0:1:0:0 --> AF4 for pin PB8
|
||||
// Bits (7:6:5:4) = 0:1:0:0 --> AF4 for pin PB9
|
||||
|
38
IODevice.cpp
38
IODevice.cpp
@@ -251,6 +251,26 @@ void IODevice::write(VPIN vpin, int value) {
|
||||
#endif
|
||||
}
|
||||
|
||||
// Write value to count virtual pin(s).
|
||||
// these may be within one driver or separated over several drivers
|
||||
void IODevice::writeRange(VPIN vpin, int value, int count) {
|
||||
|
||||
while(count) {
|
||||
auto dev = findDevice(vpin);
|
||||
if (dev) {
|
||||
auto vpinBefore=vpin;
|
||||
// write to driver, driver will return next vpin it cant handle
|
||||
vpin=dev->_writeRange(vpin, value,count);
|
||||
count-= vpin-vpinBefore; // decrement by number of vpins changed
|
||||
}
|
||||
else {
|
||||
// skip a vpin if no device handler
|
||||
vpin++;
|
||||
count--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Write analogue value to virtual pin(s). If multiple devices are allocated
|
||||
// the same pin then only the first one found will be used.
|
||||
//
|
||||
@@ -270,6 +290,24 @@ void IODevice::writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t para
|
||||
#endif
|
||||
}
|
||||
|
||||
//
|
||||
void IODevice::writeAnalogueRange(VPIN vpin, int value, uint8_t param1, uint16_t param2,int count) {
|
||||
while(count) {
|
||||
auto dev = findDevice(vpin);
|
||||
if (dev) {
|
||||
auto vpinBefore=vpin;
|
||||
// write to driver, driver will return next vpin it cant handle
|
||||
vpin=dev->_writeAnalogueRange(vpin, value, param1, param2,count);
|
||||
count-= vpin-vpinBefore; // decrement by number of vpins changed
|
||||
}
|
||||
else {
|
||||
// skip a vpin if no device handler
|
||||
vpin++;
|
||||
count--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// isBusy, when called for a device pin is always a digital output or analogue output,
|
||||
// returns input feedback state of the pin, i.e. whether the pin is busy performing
|
||||
// an animation or fade over a period of time.
|
||||
|
25
IODevice.h
25
IODevice.h
@@ -128,9 +128,11 @@ public:
|
||||
|
||||
// write invokes the IODevice instance's _write method.
|
||||
static void write(VPIN vpin, int value);
|
||||
static void writeRange(VPIN vpin, int value,int count);
|
||||
|
||||
// write invokes the IODevice instance's _writeAnalogue method (not applicable for digital outputs)
|
||||
static void writeAnalogue(VPIN vpin, int value, uint8_t profile=0, uint16_t duration=0);
|
||||
static void writeAnalogueRange(VPIN vpin, int value, uint8_t profile, uint16_t duration, int count);
|
||||
|
||||
// isBusy returns true if the device is currently in an animation of some sort, e.g. is changing
|
||||
// the output over a period of time.
|
||||
@@ -177,11 +179,29 @@ public:
|
||||
virtual void _write(VPIN vpin, int value) {
|
||||
(void)vpin; (void)value;
|
||||
};
|
||||
|
||||
// Method to write new state (optionally implemented within device class)
|
||||
// This will, by default just write to one vpin and return whet to do next.
|
||||
// the real power comes where a single driver can update many vpins in one call.
|
||||
virtual VPIN _writeRange(VPIN vpin, int value, int count) {
|
||||
(void)count;
|
||||
_write(vpin,value);
|
||||
return vpin+1; // try next vpin
|
||||
};
|
||||
|
||||
// Method to write an 'analogue' value (optionally implemented within device class)
|
||||
virtual void _writeAnalogue(VPIN vpin, int value, uint8_t param1=0, uint16_t param2=0) {
|
||||
(void)vpin; (void)value; (void) param1; (void)param2;
|
||||
};
|
||||
|
||||
// Method to write an 'analogue' value to a VPIN range (optionally implemented within device class)
|
||||
// This will, by default just write to one vpin and return whet to do next.
|
||||
// the real power comes where a single driver can update many vpins in one call.
|
||||
virtual VPIN _writeAnalogueRange(VPIN vpin, int value, uint8_t param1, uint16_t param2, int count) {
|
||||
(void) count;
|
||||
_writeAnalogue(vpin, value, param1, param2);
|
||||
return vpin+1;
|
||||
};
|
||||
|
||||
// Method to read digital pin state (optionally implemented within device class)
|
||||
virtual int _read(VPIN vpin) {
|
||||
@@ -547,6 +567,9 @@ protected:
|
||||
#include "IO_duinoNodes.h"
|
||||
#include "IO_EXIOExpander.h"
|
||||
#include "IO_trainbrains.h"
|
||||
|
||||
#include "IO_EncoderThrottle.h"
|
||||
#include "IO_TCA8418.h"
|
||||
#include "IO_NeoPixel.h"
|
||||
#include "IO_TM1638.h"
|
||||
|
||||
#endif // iodevice_h
|
||||
|
144
IO_EncoderThrottle.cpp
Normal file
144
IO_EncoderThrottle.cpp
Normal file
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
* © 2024, Chris Harlow. All rights reserved.
|
||||
*
|
||||
* This file is part of EX-CommandStation
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* The IO_EncoderThrottle device driver uses a rotary encoder connected to vpins
|
||||
* to drive a loco.
|
||||
* Loco id is selected by writeAnalog.
|
||||
*/
|
||||
|
||||
#include "IODevice.h"
|
||||
#include "DIAG.h"
|
||||
#include "DCC.h"
|
||||
|
||||
const byte _DIR_CW = 0x10; // Clockwise step
|
||||
const byte _DIR_CCW = 0x20; // Counter-clockwise step
|
||||
|
||||
const byte transition_table[5][4]= {
|
||||
{0,1,3,0}, // 0: 00
|
||||
{1,1,1,2 | _DIR_CW}, // 1: 00->01
|
||||
{2,2,0,2}, // 2: 00->01->11
|
||||
{3,3,3,4 | _DIR_CCW}, // 3: 00->10
|
||||
{4,0,4,4} // 4: 00->10->11
|
||||
};
|
||||
|
||||
const byte _STATE_MASK = 0x07;
|
||||
const byte _DIR_MASK = 0x30;
|
||||
|
||||
|
||||
|
||||
void EncoderThrottle::create(VPIN firstVpin, int dtPin, int clkPin, int clickPin, byte notch) {
|
||||
if (checkNoOverlap(firstVpin)) new EncoderThrottle(firstVpin, dtPin,clkPin,clickPin,notch);
|
||||
}
|
||||
|
||||
|
||||
// Constructor
|
||||
EncoderThrottle::EncoderThrottle(VPIN firstVpin, int dtPin, int clkPin, int clickPin, byte notch){
|
||||
_firstVpin = firstVpin;
|
||||
_nPins = 1;
|
||||
_I2CAddress = 0;
|
||||
_dtPin=dtPin;
|
||||
_clkPin=clkPin;
|
||||
_clickPin=clickPin;
|
||||
_notch=notch;
|
||||
_locoid=0;
|
||||
_stopState=xrSTOP;
|
||||
_rocoState=0;
|
||||
_prevpinstate=4; // not 01..11
|
||||
IODevice::configureInput(dtPin,true);
|
||||
IODevice::configureInput(clkPin,true);
|
||||
IODevice::configureInput(clickPin,true);
|
||||
addDevice(this);
|
||||
_display();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void EncoderThrottle::_loop(unsigned long currentMicros) {
|
||||
if (_locoid==0) return; // not in use
|
||||
|
||||
// Clicking down on the roco, stops the loco and sets the direction as unknown.
|
||||
if (IODevice::read(_clickPin)) {
|
||||
if (_stopState==xrSTOP) return; // debounced multiple stops
|
||||
DCC::setThrottle(_locoid,1,DCC::getThrottleDirection(_locoid));
|
||||
_stopState=xrSTOP;
|
||||
DIAG(F("DRIVE %d STOP"),_locoid);
|
||||
return;
|
||||
}
|
||||
|
||||
// read roco pins and detect state change
|
||||
byte pinstate = (IODevice::read(_dtPin) << 1) | IODevice::read(_clkPin);
|
||||
if (pinstate==_prevpinstate) return;
|
||||
_prevpinstate=pinstate;
|
||||
|
||||
_rocoState = transition_table[_rocoState & _STATE_MASK][pinstate];
|
||||
if ((_rocoState & _DIR_MASK) == 0) return; // no value change
|
||||
|
||||
int change=(_rocoState & _DIR_CW)?+1:-1;
|
||||
// handle roco change -1 or +1 (clockwise)
|
||||
|
||||
if (_stopState==xrSTOP) {
|
||||
// first move after button press sets the direction. (clockwise=fwd)
|
||||
_stopState=change>0?xrFWD:xrREV;
|
||||
}
|
||||
|
||||
// when going fwd, clockwise increases speed.
|
||||
// but when reversing, anticlockwise increases speed.
|
||||
// This is similar to a center-zero pot control but with
|
||||
// the added safety that you cant panic-spin into the other
|
||||
// direction.
|
||||
if (_stopState==xrREV) change=-change;
|
||||
// manage limits
|
||||
int oldspeed=DCC::getThrottleSpeed(_locoid);
|
||||
if (oldspeed==1)oldspeed=0; // break out of estop
|
||||
int newspeed=change>0 ? (min((oldspeed+_notch),126)) : (max(0,(oldspeed-_notch)));
|
||||
if (newspeed==1) newspeed=0; // normal decelereated stop.
|
||||
if (oldspeed!=newspeed) {
|
||||
DIAG(F("DRIVE %d notch %S %d %S"),_locoid,
|
||||
change>0?F("UP"):F("DOWN"),_notch,
|
||||
_stopState==xrFWD?F("FWD"):F("REV"));
|
||||
DCC::setThrottle(_locoid,newspeed,_stopState==xrFWD);
|
||||
}
|
||||
}
|
||||
|
||||
// Selocoid as analog value to start drive
|
||||
// use <z vpin locoid [notch]>
|
||||
void EncoderThrottle::_writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) {
|
||||
(void) param2;
|
||||
_locoid=value;
|
||||
if (param1>0) _notch=param1;
|
||||
_rocoState=0;
|
||||
|
||||
// If loco is moving, we inherit direction from it.
|
||||
_stopState=xrSTOP;
|
||||
if (_locoid>0) {
|
||||
auto speedbyte=DCC::getThrottleSpeedByte(_locoid);
|
||||
if ((speedbyte & 0x7f) >1) {
|
||||
// loco is moving
|
||||
_stopState= (speedbyte & 0x80)?xrFWD:xrREV;
|
||||
}
|
||||
}
|
||||
_display();
|
||||
}
|
||||
|
||||
|
||||
void EncoderThrottle::_display() {
|
||||
DIAG(F("DRIVE vpin %d loco %d notch %d"),_firstVpin,_locoid,_notch);
|
||||
}
|
||||
|
53
IO_EncoderThrottle.h
Normal file
53
IO_EncoderThrottle.h
Normal file
@@ -0,0 +1,53 @@
|
||||
/*
|
||||
* © 2024, Chris Harlow. All rights reserved.
|
||||
*
|
||||
* This file is part of EX-CommandStation
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* The IO_EncoderThrottle device driver uses a rotary encoder connected to vpins
|
||||
* to drive a loco.
|
||||
* Loco id is selected by writeAnalog.
|
||||
*/
|
||||
|
||||
#ifndef IO_EncoderThrottle_H
|
||||
#define IO_EncoderThrottle_H
|
||||
#include "IODevice.h"
|
||||
|
||||
class EncoderThrottle : public IODevice {
|
||||
public:
|
||||
|
||||
static void create(VPIN firstVpin, int dtPin, int clkPin, int clickPin, byte notch=10);
|
||||
|
||||
private:
|
||||
int _dtPin,_clkPin,_clickPin, _locoid, _notch,_prevpinstate;
|
||||
enum {xrSTOP,xrFWD,xrREV} _stopState;
|
||||
byte _rocoState;
|
||||
|
||||
// Constructor
|
||||
EncoderThrottle(VPIN firstVpin, int dtPin, int clkPin, int clickPin, byte notch);
|
||||
|
||||
void _loop(unsigned long currentMicros) override ;
|
||||
|
||||
// Selocoid as analog value to start drive
|
||||
// use <z vpin locoid [notch]>
|
||||
void _writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) override;
|
||||
|
||||
void _display() override ;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
333
IO_NeoPixel.h
Normal file
333
IO_NeoPixel.h
Normal file
@@ -0,0 +1,333 @@
|
||||
/*
|
||||
* © 2024, Chris Harlow. All rights reserved.
|
||||
*
|
||||
* This file is part of EX-CommandStation
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* The IO_NEOPIXEL.h device driver integrates with one or more Adafruit neopixel drivers.
|
||||
* This device driver will configure the device on startup, along with
|
||||
* interacting with the device for all input/output duties.
|
||||
*
|
||||
* To create NEOPIXEL devices, these are defined in myAutomation.h:
|
||||
* (Note the device driver is included by default)
|
||||
*
|
||||
* HAL(NEOPIXEL,first vpin, number of pixels,mode, i2c address)
|
||||
* e.g. HAL(NEOPIXEL,1000,64,NEO_RGB,0x60)
|
||||
* This gives each pixel in the chain an individual vpin
|
||||
* The number of pixels must match the physical pixels in the chain.
|
||||
*
|
||||
* This driver maintains a colour (rgb value in 5,5,5 bits only) plus an ON bit.
|
||||
* This can be written/read with an analog write/read call.
|
||||
* The ON bit can be set on and off with a digital write. This allows for
|
||||
* a pixel to be preset a colour and then turned on and off like any other light.
|
||||
*/
|
||||
|
||||
#ifndef IO_EX_NeoPixel_H
|
||||
#define IO_EX_NeoPixel_H
|
||||
|
||||
#include "IODevice.h"
|
||||
#include "I2CManager.h"
|
||||
#include "DIAG.h"
|
||||
#include "FSH.h"
|
||||
|
||||
|
||||
// The following macros to define the Neopixel String type
|
||||
// have been copied from the Adafruit Seesaw Library under the
|
||||
// terms of the GPL.
|
||||
// Credit to: https://github.com/adafruit/Adafruit_Seesaw
|
||||
|
||||
// The order of primary colors in the NeoPixel data stream can vary
|
||||
// among device types, manufacturers and even different revisions of
|
||||
// the same item. The third parameter to the seesaw_NeoPixel
|
||||
// constructor encodes the per-pixel byte offsets of the red, green
|
||||
// and blue primaries (plus white, if present) in the data stream --
|
||||
// the following #defines provide an easier-to-use named version for
|
||||
// each permutation. e.g. NEO_GRB indicates a NeoPixel-compatible
|
||||
// device expecting three bytes per pixel, with the first byte
|
||||
// containing the green value, second containing red and third
|
||||
// containing blue. The in-memory representation of a chain of
|
||||
// NeoPixels is the same as the data-stream order; no re-ordering of
|
||||
// bytes is required when issuing data to the chain.
|
||||
|
||||
// Bits 5,4 of this value are the offset (0-3) from the first byte of
|
||||
// a pixel to the location of the red color byte. Bits 3,2 are the
|
||||
// green offset and 1,0 are the blue offset. If it is an RGBW-type
|
||||
// device (supporting a white primary in addition to R,G,B), bits 7,6
|
||||
// are the offset to the white byte...otherwise, bits 7,6 are set to
|
||||
// the same value as 5,4 (red) to indicate an RGB (not RGBW) device.
|
||||
// i.e. binary representation:
|
||||
// 0bWWRRGGBB for RGBW devices
|
||||
// 0bRRRRGGBB for RGB
|
||||
|
||||
// RGB NeoPixel permutations; white and red offsets are always same
|
||||
// Offset: W R G B
|
||||
#define NEO_RGB ((0 << 6) | (0 << 4) | (1 << 2) | (2))
|
||||
#define NEO_RBG ((0 << 6) | (0 << 4) | (2 << 2) | (1))
|
||||
#define NEO_GRB ((1 << 6) | (1 << 4) | (0 << 2) | (2))
|
||||
#define NEO_GBR ((2 << 6) | (2 << 4) | (0 << 2) | (1))
|
||||
#define NEO_BRG ((1 << 6) | (1 << 4) | (2 << 2) | (0))
|
||||
#define NEO_BGR ((2 << 6) | (2 << 4) | (1 << 2) | (0))
|
||||
|
||||
// RGBW NeoPixel permutations; all 4 offsets are distinct
|
||||
// Offset: W R G B
|
||||
#define NEO_WRGB ((0 << 6) | (1 << 4) | (2 << 2) | (3))
|
||||
#define NEO_WRBG ((0 << 6) | (1 << 4) | (3 << 2) | (2))
|
||||
#define NEO_WGRB ((0 << 6) | (2 << 4) | (1 << 2) | (3))
|
||||
#define NEO_WGBR ((0 << 6) | (3 << 4) | (1 << 2) | (2))
|
||||
#define NEO_WBRG ((0 << 6) | (2 << 4) | (3 << 2) | (1))
|
||||
#define NEO_WBGR ((0 << 6) | (3 << 4) | (2 << 2) | (1))
|
||||
|
||||
#define NEO_RWGB ((1 << 6) | (0 << 4) | (2 << 2) | (3))
|
||||
#define NEO_RWBG ((1 << 6) | (0 << 4) | (3 << 2) | (2))
|
||||
#define NEO_RGWB ((2 << 6) | (0 << 4) | (1 << 2) | (3))
|
||||
#define NEO_RGBW ((3 << 6) | (0 << 4) | (1 << 2) | (2))
|
||||
#define NEO_RBWG ((2 << 6) | (0 << 4) | (3 << 2) | (1))
|
||||
#define NEO_RBGW ((3 << 6) | (0 << 4) | (2 << 2) | (1))
|
||||
|
||||
#define NEO_GWRB ((1 << 6) | (2 << 4) | (0 << 2) | (3))
|
||||
#define NEO_GWBR ((1 << 6) | (3 << 4) | (0 << 2) | (2))
|
||||
#define NEO_GRWB ((2 << 6) | (1 << 4) | (0 << 2) | (3))
|
||||
#define NEO_GRBW ((3 << 6) | (1 << 4) | (0 << 2) | (2))
|
||||
#define NEO_GBWR ((2 << 6) | (3 << 4) | (0 << 2) | (1))
|
||||
#define NEO_GBRW ((3 << 6) | (2 << 4) | (0 << 2) | (1))
|
||||
|
||||
#define NEO_BWRG ((1 << 6) | (2 << 4) | (3 << 2) | (0))
|
||||
#define NEO_BWGR ((1 << 6) | (3 << 4) | (2 << 2) | (0))
|
||||
#define NEO_BRWG ((2 << 6) | (1 << 4) | (3 << 2) | (0))
|
||||
#define NEO_BRGW ((3 << 6) | (1 << 4) | (2 << 2) | (0))
|
||||
#define NEO_BGWR ((2 << 6) | (3 << 4) | (1 << 2) | (0))
|
||||
#define NEO_BGRW ((3 << 6) | (2 << 4) | (1 << 2) | (0))
|
||||
|
||||
// If 400 KHz support is enabled, the third parameter to the constructor
|
||||
// requires a 16-bit value (in order to select 400 vs 800 KHz speed).
|
||||
// If only 800 KHz is enabled (as is default on ATtiny), an 8-bit value
|
||||
// is sufficient to encode pixel color order, saving some space.
|
||||
|
||||
#define NEO_KHZ800 0x0000 // 800 KHz datastream
|
||||
#define NEO_KHZ400 0x0100 // 400 KHz datastream
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/*
|
||||
* IODevice subclass for NeoPixel.
|
||||
*/
|
||||
|
||||
class NeoPixel : public IODevice {
|
||||
public:
|
||||
|
||||
static void create(VPIN vpin, int nPins, uint16_t mode=(NEO_GRB | NEO_KHZ800), I2CAddress i2cAddress=0x60) {
|
||||
if (checkNoOverlap(vpin, nPins, mode, i2cAddress)) new NeoPixel(vpin, nPins, mode, i2cAddress);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
static const byte SEESAW_NEOPIXEL_BASE=0x0E;
|
||||
static const byte SEESAW_NEOPIXEL_STATUS = 0x00;
|
||||
static const byte SEESAW_NEOPIXEL_PIN = 0x01;
|
||||
static const byte SEESAW_NEOPIXEL_SPEED = 0x02;
|
||||
static const byte SEESAW_NEOPIXEL_BUF_LENGTH = 0x03;
|
||||
static const byte SEESAW_NEOPIXEL_BUF=0x04;
|
||||
static const byte SEESAW_NEOPIXEL_SHOW=0x05;
|
||||
|
||||
// all adafruit examples say this pin. Presumably its hard wired
|
||||
// in the adapter anyway.
|
||||
static const byte SEESAW_PIN15 = 15;
|
||||
|
||||
// Constructor
|
||||
NeoPixel(VPIN firstVpin, int nPins, uint16_t mode, I2CAddress i2cAddress) {
|
||||
_firstVpin = firstVpin;
|
||||
_nPins=nPins;
|
||||
_I2CAddress = i2cAddress;
|
||||
|
||||
// calculate the offsets into the seesaw buffer for each colour depending
|
||||
// on the pixel strip type passed in mode.
|
||||
|
||||
_redOffset=4+(mode >> 4 & 0x03);
|
||||
_greenOffset=4+(mode >> 2 & 0x03);
|
||||
_blueOffset=4+(mode & 0x03);
|
||||
if (4+(mode >>6 & 0x03) == _redOffset) _bytesPerPixel=3;
|
||||
else _bytesPerPixel=4; // string has a white byte.
|
||||
|
||||
_kHz800=(mode & NEO_KHZ400)==0;
|
||||
_showPendimg=false;
|
||||
|
||||
// Each pixel requires 3 bytes RGB memory.
|
||||
// Although the driver device can remember this, it cant do off/on without
|
||||
// forgetting what the on colour was!
|
||||
pixelBuffer=(RGB *) malloc(_nPins*sizeof(RGB));
|
||||
stateBuffer=(byte *) calloc((_nPins+7)/8,sizeof(byte)); // all pixels off
|
||||
if (pixelBuffer==nullptr || stateBuffer==nullptr) {
|
||||
DIAG(F("NeoPixel I2C:%s not enough RAM"), _I2CAddress.toString());
|
||||
return;
|
||||
}
|
||||
// preset all pins to white so a digital on/off will do something even if no colour set.
|
||||
memset(pixelBuffer,0xFF,_nPins*sizeof(RGB));
|
||||
addDevice(this);
|
||||
}
|
||||
|
||||
void _begin() {
|
||||
|
||||
// Initialise Neopixel device
|
||||
I2CManager.begin();
|
||||
if (!I2CManager.exists(_I2CAddress)) {
|
||||
DIAG(F("NeoPixel I2C:%s device not found"), _I2CAddress.toString());
|
||||
_deviceState = DEVSTATE_FAILED;
|
||||
return;
|
||||
}
|
||||
|
||||
byte speedBuffer[]={SEESAW_NEOPIXEL_BASE, SEESAW_NEOPIXEL_SPEED,_kHz800};
|
||||
I2CManager.write(_I2CAddress, speedBuffer, sizeof(speedBuffer));
|
||||
|
||||
// In the driver there are 3 of 4 byts per pixel
|
||||
auto numBytes=_bytesPerPixel * _nPins;
|
||||
byte setbuffer[] = {SEESAW_NEOPIXEL_BASE, SEESAW_NEOPIXEL_BUF_LENGTH,
|
||||
(byte)(numBytes >> 8), (byte)(numBytes & 0xFF)};
|
||||
I2CManager.write(_I2CAddress, setbuffer, sizeof(setbuffer));
|
||||
|
||||
const byte pinbuffer[] = {SEESAW_NEOPIXEL_BASE, SEESAW_NEOPIXEL_PIN,SEESAW_PIN15};
|
||||
I2CManager.write(_I2CAddress, pinbuffer, sizeof(pinbuffer));
|
||||
|
||||
for (auto pin=0;pin<_nPins;pin++) transmit(pin);
|
||||
_display();
|
||||
}
|
||||
|
||||
// loop called by HAL supervisor
|
||||
void _loop(unsigned long currentMicros) override {
|
||||
if (!_showPendimg) return;
|
||||
byte showBuffer[]={SEESAW_NEOPIXEL_BASE,SEESAW_NEOPIXEL_SHOW};
|
||||
I2CManager.write(_I2CAddress,showBuffer,sizeof(showBuffer));
|
||||
_showPendimg=false;
|
||||
}
|
||||
|
||||
|
||||
// read back pixel on/off
|
||||
int _read(VPIN vpin) override {
|
||||
if (_deviceState == DEVSTATE_FAILED) return 0;
|
||||
return isPixelOn(vpin-_firstVpin);
|
||||
}
|
||||
|
||||
// Write digital value. Sets pixel on or off
|
||||
void _write(VPIN vpin, int value) override {
|
||||
if (_deviceState == DEVSTATE_FAILED) return;
|
||||
auto pixel=vpin-_firstVpin;
|
||||
if (value) {
|
||||
if (isPixelOn(pixel)) return;
|
||||
setPixelOn(pixel);
|
||||
}
|
||||
else { // set off
|
||||
if (!isPixelOn(pixel)) return;
|
||||
setPixelOff(pixel);
|
||||
}
|
||||
transmit(pixel);
|
||||
}
|
||||
|
||||
VPIN _writeRange(VPIN vpin,int value, int count) {
|
||||
// using write range cuts out the constant vpin to driver lookup so
|
||||
// we can update multiple pixels much faster.
|
||||
VPIN nextVpin=vpin + (count>_nPins ? _nPins : count);
|
||||
if (_deviceState != DEVSTATE_FAILED) while(vpin<nextVpin) {
|
||||
_write(vpin,value);
|
||||
vpin++;
|
||||
}
|
||||
return nextVpin; // next pin we cant
|
||||
}
|
||||
// Write analogue value.
|
||||
// The convoluted parameter mashing here is to allow passing the RGB and on/off
|
||||
// information through the generic HAL _writeAnalog interface which was originally
|
||||
// designed for servos and short integers
|
||||
void _writeAnalogue(VPIN vpin, int colour_RG, uint8_t onoff, uint16_t colour_B) override {
|
||||
if (_deviceState == DEVSTATE_FAILED) return;
|
||||
RGB newColour={(byte)((colour_RG>>8) & 0xFF), (byte)(colour_RG & 0xFF), (byte)(colour_B & 0xFF)};
|
||||
auto pixel=vpin-_firstVpin;
|
||||
if (pixelBuffer[pixel]==newColour && isPixelOn(pixel)==(bool)onoff) return; // no change
|
||||
|
||||
if (onoff) setPixelOn(pixel); else setPixelOff(pixel);
|
||||
pixelBuffer[pixel]=newColour;
|
||||
transmit(pixel);
|
||||
}
|
||||
VPIN _writeAnalogueRange(VPIN vpin, int colour_RG, uint8_t onoff, uint16_t colour_B, int count) override {
|
||||
// using write range cuts out the constant vpin to driver lookup so
|
||||
VPIN nextVpin=vpin + (count>_nPins ? _nPins : count);
|
||||
if (_deviceState != DEVSTATE_FAILED) while(vpin<nextVpin) {
|
||||
_writeAnalogue(vpin,colour_RG, onoff,colour_B);
|
||||
vpin++;
|
||||
}
|
||||
return nextVpin; // next pin we cant
|
||||
}
|
||||
|
||||
// Display device information and status.
|
||||
void _display() override {
|
||||
DIAG(F("NeoPixel I2C:%s Vpins %u-%u %S"),
|
||||
_I2CAddress.toString(),
|
||||
(int)_firstVpin, (int)_firstVpin+_nPins-1,
|
||||
_deviceState == DEVSTATE_FAILED ? F("OFFLINE") : F(""));
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool isPixelOn(int16_t pixel) {return stateBuffer[pixel/8] & (0x80>>(pixel%8));}
|
||||
void setPixelOn(int16_t pixel) {stateBuffer[pixel/8] |= (0x80>>(pixel%8));}
|
||||
void setPixelOff(int16_t pixel) {stateBuffer[pixel/8] &= ~(0x80>>(pixel%8));}
|
||||
|
||||
// Helper function for error handling
|
||||
void reportError(uint8_t status, bool fail=true) {
|
||||
DIAG(F("NeoPixel I2C:%s Error:%d (%S)"), _I2CAddress.toString(),
|
||||
status, I2CManager.getErrorMessage(status));
|
||||
if (fail)
|
||||
_deviceState = DEVSTATE_FAILED;
|
||||
}
|
||||
|
||||
|
||||
void transmit(uint16_t pixel, bool show=true) {
|
||||
byte buffer[]={SEESAW_NEOPIXEL_BASE,SEESAW_NEOPIXEL_BUF,0x00,0x00,0x00,0x00,0x00};
|
||||
uint16_t offset= pixel * _bytesPerPixel;
|
||||
buffer[2]=(byte)(offset>>8);
|
||||
buffer[3]=(byte)(offset & 0xFF);
|
||||
|
||||
if (isPixelOn(pixel)) {
|
||||
auto colour=pixelBuffer[pixel];
|
||||
buffer[_redOffset]=colour.red;
|
||||
buffer[_greenOffset]=colour.green;
|
||||
buffer[_blueOffset]=colour.blue;
|
||||
} // else leave buffer black (in buffer preset to zeros above)
|
||||
|
||||
// Transmit pixel to driver
|
||||
I2CManager.write(_I2CAddress,buffer,4 +_bytesPerPixel);
|
||||
_showPendimg=true;
|
||||
|
||||
}
|
||||
struct RGB {
|
||||
byte red;
|
||||
byte green;
|
||||
byte blue;
|
||||
bool operator==(const RGB& other) const {
|
||||
return red == other.red && green == other.green && blue == other.blue;
|
||||
}
|
||||
};
|
||||
|
||||
RGB* pixelBuffer = nullptr;
|
||||
byte* stateBuffer = nullptr; // 1 bit per pixel
|
||||
bool _showPendimg;
|
||||
|
||||
// mapping of RGB onto pixel buffer for seesaw.
|
||||
byte _bytesPerPixel;
|
||||
byte _redOffset;
|
||||
byte _greenOffset;
|
||||
byte _blueOffset;
|
||||
bool _kHz800;
|
||||
};
|
||||
|
||||
#endif
|
184
IO_TCA8418.h
Normal file
184
IO_TCA8418.h
Normal file
@@ -0,0 +1,184 @@
|
||||
/*
|
||||
* © 2023, Paul M. Antoine
|
||||
* © 2021, Neil McKechnie. All rights reserved.
|
||||
*
|
||||
* This file is part of DCC-EX API
|
||||
*
|
||||
* 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 io_tca8418_h
|
||||
#define io_tca8418_h
|
||||
|
||||
#include "IO_GPIOBase.h"
|
||||
#include "FSH.h"
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/*
|
||||
* IODevice subclass for TCA8418 80-key keypad encoder, which we'll treat as 64 of the possible
|
||||
* 80 inputs for now, in an 8x8 matrix only, although the datasheet says:
|
||||
*
|
||||
* The TCA8418 can be configured to support many different configurations of keypad setups.
|
||||
* All 18 GPIOs for the rows and columns can be used to support up to 80 keys in an 8x10 key pad
|
||||
* array. Another option is that all 18 GPIOs be used for GPIs to read 18 buttons which are
|
||||
* not connected in an array. Any combination in between is also acceptable (for example, a
|
||||
* 3x4 keypad matrix and using the remaining 11 GPIOs as a combination of inputs and outputs).
|
||||
*/
|
||||
|
||||
class TCA8418 : public GPIOBase<uint64_t> {
|
||||
public:
|
||||
static void create(VPIN vpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1) {
|
||||
if (checkNoOverlap(vpin, nPins, i2cAddress))
|
||||
// temporarily use the simple 18-pin GPIO mode - we'll switch to 8x8 matrix once this works
|
||||
new TCA8418(vpin, (nPins = (nPins > 18) ? 18 : nPins), i2cAddress, interruptPin);
|
||||
}
|
||||
|
||||
private:
|
||||
// Constructor
|
||||
TCA8418(VPIN vpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1)
|
||||
: GPIOBase<uint64_t>((FSH *)F("TCA8418"), vpin, nPins, i2cAddress, interruptPin)
|
||||
{
|
||||
uint8_t receiveBuffer[1];
|
||||
uint8_t commandBuffer[1];
|
||||
uint8_t status;
|
||||
|
||||
commandBuffer[0] = REG_INT_STAT; // Check interrupt status
|
||||
status = I2CManager.read(_I2CAddress, receiveBuffer, sizeof(receiveBuffer), commandBuffer, sizeof(commandBuffer));
|
||||
if (status == I2C_STATUS_OK) {
|
||||
DIAG(F("TCA8418 Interrupt status was: %x"), receiveBuffer[0]);
|
||||
}
|
||||
else
|
||||
DIAG(F("TCA8418 Interrupt status failed to read!"));
|
||||
// requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer),
|
||||
// outputBuffer, sizeof(outputBuffer));
|
||||
// outputBuffer[0] = REG_GPIOA;
|
||||
}
|
||||
void _writeGpioPort() override {
|
||||
// I2CManager.write(_I2CAddress, 3, REG_GPIOA, _portOutputState, _portOutputState>>8);
|
||||
}
|
||||
void _writePullups() override {
|
||||
// Set pullups only for in-use pins. This prevents pullup being set for a pin that
|
||||
// is intended for use as an output but hasn't been written to yet.
|
||||
uint32_t temp = _portPullup & _portInUse;
|
||||
(void)temp; // Chris did this so he could see warnings that mattered
|
||||
// I2CManager.write(_I2CAddress, 3, REG_GPPUA, temp, temp>>8);
|
||||
}
|
||||
void _writePortModes() override {
|
||||
// Write 0 to each GPIO_DIRn for in-use pins that are inputs, 1 for outputs
|
||||
uint64_t temp = _portMode & _portInUse;
|
||||
DIAG(F("TCA8418 writing Port Mode: %x, to GPIO_DIRs"), temp);
|
||||
DIAG(F("TCA8418 writing Port Mode: %x, to GPIO_DIR1"), (temp&0xFF));
|
||||
I2CManager.write(_I2CAddress, 2, REG_GPIO_DIR1, (temp&0xFF));
|
||||
DIAG(F("TCA8418 writing Port Mode: %x, to GPIO_DIR2"), ((temp&0xFF00)>>8));
|
||||
I2CManager.write(_I2CAddress, 2, REG_GPIO_DIR2, ((temp&0xFF00)>>8));
|
||||
DIAG(F("TCA8418 writing Port Mode: %x, to GPIO_DIR3"), (temp&0x30000)>>16);
|
||||
I2CManager.write(_I2CAddress, 2, REG_GPIO_DIR3, ((temp&0x30000)>>16));
|
||||
|
||||
// Enable interrupt for in-use pins which are inputs (_portMode=0)
|
||||
// TCA8418 has interrupt enables per pin, but must be configured for low->high
|
||||
// or high->low... unlike the MCP23017
|
||||
temp = ~_portMode & _portInUse;
|
||||
DIAG(F("TCA8418 writing interrupt Port Mode: %x, to GPIO_INT_ENs"), temp);
|
||||
DIAG(F("TCA8418 writing interrupt Port Mode: %x, to GPIO_INT_EN1"), (temp&0xFF));
|
||||
I2CManager.write(_I2CAddress, 2, REG_GPIO_INT_EN1, (temp&0xFF));
|
||||
DIAG(F("TCA8418 writing interrupt Port Mode: %x, to GPIO_INT_EN2"), ((temp&0xFF00)>>8));
|
||||
I2CManager.write(_I2CAddress, 2, REG_GPIO_INT_EN2, ((temp&0xFF00)>>8));
|
||||
DIAG(F("TCA8418 writing interrupt Port Mode: %x, to GPIO_INT_EN3"), (temp&0x30000)>>16);
|
||||
I2CManager.write(_I2CAddress, 2, REG_GPIO_INT_EN3, ((temp&0x30000)>>16));
|
||||
// I2CManager.write(_I2CAddress, 3, REG_INTCONA, 0x00, 0x00);
|
||||
// I2CManager.write(_I2CAddress, 3, REG_GPINTENA, temp, temp>>8);
|
||||
}
|
||||
void _readGpioPort(bool immediate) override {
|
||||
// if (immediate) {
|
||||
// uint8_t buffer[2];
|
||||
// I2CManager.read(_I2CAddress, buffer, 2, 1, REG_GPIOA);
|
||||
// _portInputState = ((uint16_t)buffer[1]<<8) | buffer[0] | _portMode;
|
||||
// } else {
|
||||
// // Queue new request
|
||||
// requestBlock.wait(); // Wait for preceding operation to complete
|
||||
// // Issue new request to read GPIO register
|
||||
// I2CManager.queueRequest(&requestBlock);
|
||||
// }
|
||||
}
|
||||
// This function is invoked when an I/O operation on the requestBlock completes.
|
||||
void _processCompletion(uint8_t status) override {
|
||||
// if (status == I2C_STATUS_OK)
|
||||
// _portInputState = (((uint16_t)inputBuffer[1]<<8) | inputBuffer[0]) | _portMode;
|
||||
// else
|
||||
// _portInputState = 0xffff;
|
||||
}
|
||||
|
||||
void _setupDevice() override {
|
||||
DIAG(F("TCA8418 setupDevice() called"));
|
||||
// IOCON is set MIRROR=1, ODR=1 (open drain shared interrupt pin)
|
||||
// I2CManager.write(_I2CAddress, 2, REG_IOCON, 0x44);
|
||||
_writePortModes();
|
||||
_writePullups();
|
||||
_writeGpioPort();
|
||||
}
|
||||
|
||||
enum
|
||||
{
|
||||
REG_FIRST_RESERVED = 0x00,
|
||||
REG_CFG = 0x01,
|
||||
REG_INT_STAT = 0x02,
|
||||
REG_KEY_LCK_EC = 0x03,
|
||||
REG_KEY_EVENT_A = 0x04,
|
||||
REG_KEY_EVENT_B = 0x05,
|
||||
REG_KEY_EVENT_C = 0x06,
|
||||
REG_KEY_EVENT_D = 0x07,
|
||||
REG_KEY_EVENT_E = 0x08,
|
||||
REG_KEY_EVENT_F = 0x09,
|
||||
REG_KEY_EVENT_G = 0x0A,
|
||||
REG_KEY_EVENT_H = 0x0B,
|
||||
REG_KEY_EVENT_I = 0x0C,
|
||||
REG_KEY_EVENT_J = 0x0D,
|
||||
REG_KP_LCK_TIMER = 0x0E,
|
||||
REG_UNLOCK1 = 0x0F,
|
||||
REG_UNLOCK2 = 0x10,
|
||||
REG_GPIO_INT_STAT1 = 0x11,
|
||||
REG_GPIO_INT_STAT2 = 0x12,
|
||||
REG_GPIO_INT_STAT3 = 0x13,
|
||||
REG_GPIO_DAT_STAT1 = 0x14,
|
||||
REG_GPIO_DAT_STAT2 = 0x15,
|
||||
REG_GPIO_DAT_STAT3 = 0x16,
|
||||
REG_GPIO_DAT_OUT1 = 0x17,
|
||||
REG_GPIO_DAT_OUT2 = 0x18,
|
||||
REG_GPIO_DAT_OUT3 = 0x19,
|
||||
REG_GPIO_INT_EN1 = 0x1A,
|
||||
REG_GPIO_INT_EN2 = 0x1B,
|
||||
REG_GPIO_INT_EN3 = 0x1C,
|
||||
REG_KP_GPIO1 = 0x1D,
|
||||
REG_KP_GPIO2 = 0x1E,
|
||||
REG_KP_GPIO3 = 0x1F,
|
||||
REG_GPI_EM1 = 0x20,
|
||||
REG_GPI_EM2 = 0x21,
|
||||
REG_GPI_EM3 = 0x22,
|
||||
REG_GPIO_DIR1 = 0x23,
|
||||
REG_GPIO_DIR2 = 0x24,
|
||||
REG_GPIO_DIR3 = 0x25,
|
||||
REG_GPIO_INT_LVL1 = 0x26,
|
||||
REG_GPIO_INT_LVL2 = 0x27,
|
||||
REG_GPIO_INT_LVL3 = 0x28,
|
||||
REG_DEBOUNCE_DIS1 = 0x29,
|
||||
REG_DEBOUNCE_DIS2 = 0x2A,
|
||||
REG_DEBOUNCE_DIS3 = 0x2B,
|
||||
REG_GPIO_PULL1 = 0x2C,
|
||||
REG_GPIO_PULL2 = 0x2D,
|
||||
REG_GPIO_PULL3 = 0x2E,
|
||||
REG_LAST_RESERVED = 0x2F,
|
||||
};
|
||||
};
|
||||
|
||||
#endif
|
217
IO_TM1638.cpp
Normal file
217
IO_TM1638.cpp
Normal file
@@ -0,0 +1,217 @@
|
||||
/*
|
||||
* © 2024, Chris Harlow. All rights reserved.
|
||||
*
|
||||
* This file is part of DCC++EX API
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
/* Credit to https://github.com/dvarrel/TM1638 for the basic formulae.*/
|
||||
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "IODevice.h"
|
||||
#include "DIAG.h"
|
||||
|
||||
|
||||
const uint8_t HIGHFLASH _digits[16]={
|
||||
0b00111111,0b00000110,0b01011011,0b01001111,
|
||||
0b01100110,0b01101101,0b01111101,0b00000111,
|
||||
0b01111111,0b01101111,0b01110111,0b01111100,
|
||||
0b00111001,0b01011110,0b01111001,0b01110001
|
||||
};
|
||||
|
||||
// Constructor
|
||||
TM1638::TM1638(VPIN firstVpin, byte clk_pin,byte dio_pin,byte stb_pin){
|
||||
_firstVpin = firstVpin;
|
||||
_nPins = 8;
|
||||
_clk_pin = clk_pin;
|
||||
_stb_pin = stb_pin;
|
||||
_dio_pin = dio_pin;
|
||||
pinMode(clk_pin,OUTPUT);
|
||||
pinMode(stb_pin,OUTPUT);
|
||||
pinMode(dio_pin,OUTPUT);
|
||||
_pulse = PULSE1_16;
|
||||
|
||||
_buttons=0;
|
||||
_leds=0;
|
||||
_lastLoop=micros();
|
||||
addDevice(this);
|
||||
}
|
||||
|
||||
|
||||
void TM1638::create(VPIN firstVpin, byte clk_pin,byte dio_pin,byte stb_pin) {
|
||||
if (checkNoOverlap(firstVpin,8))
|
||||
new TM1638(firstVpin, clk_pin,dio_pin,stb_pin);
|
||||
}
|
||||
|
||||
void TM1638::_begin() {
|
||||
displayClear();
|
||||
test();
|
||||
_display();
|
||||
}
|
||||
|
||||
|
||||
void TM1638::_loop(unsigned long currentMicros) {
|
||||
if (currentMicros - _lastLoop > (1000000UL/LoopHz)) {
|
||||
_buttons=getButtons();// Read the buttons
|
||||
_lastLoop=currentMicros;
|
||||
}
|
||||
}
|
||||
|
||||
void TM1638::_display() {
|
||||
DIAG(F("TM1638 Configured on Vpins:%u-%u"), _firstVpin, _firstVpin+_nPins-1);
|
||||
}
|
||||
|
||||
// digital read gets button state
|
||||
int TM1638::_read(VPIN vpin) {
|
||||
byte pin=vpin - _firstVpin;
|
||||
bool result=bitRead(_buttons,pin);
|
||||
// DIAG(F("TM1638 read (%d) buttons %x = %d"),pin,_buttons,result);
|
||||
return result;
|
||||
}
|
||||
|
||||
// digital write sets led state
|
||||
void TM1638::_write(VPIN vpin, int value) {
|
||||
// TODO.. skip if no state change
|
||||
writeLed(vpin - _firstVpin + 1,value!=0);
|
||||
}
|
||||
|
||||
// Analog write sets digit displays
|
||||
|
||||
void TM1638::_writeAnalogue(VPIN vpin, int lowBytes, uint8_t mode, uint16_t highBytes) {
|
||||
// mode is in DataFormat defined above.
|
||||
byte formatLength=mode & 0x0F; // last 4 bits
|
||||
byte formatType=mode & 0xF0; //
|
||||
int8_t leftDigit=vpin-_firstVpin; // 0..7 from left
|
||||
int8_t rightDigit=leftDigit+formatLength-1; // 0..7 from left
|
||||
|
||||
// loading is done right to left startDigit first
|
||||
int8_t startDigit=7-rightDigit; // reverse as 7 on left
|
||||
int8_t lastDigit=7-leftDigit; // reverse as 7 on left
|
||||
uint32_t value=highBytes;
|
||||
value<<=16;
|
||||
value |= (uint16_t)lowBytes;
|
||||
|
||||
//DIAG(F("TM1638 fl=%d ft=%x sd=%d ld=%d v=%l vx=%X"),
|
||||
// formatLength,formatType,startDigit,lastDigit,value,value);
|
||||
while(startDigit<=lastDigit) {
|
||||
switch (formatType) {
|
||||
case _DF_DECIMAL:// decimal (leading zeros)
|
||||
displayDig(startDigit,GETHIGHFLASH(_digits,(value%10)));
|
||||
value=value/10;
|
||||
break;
|
||||
case _DF_HEX:// HEX (leading zeros)
|
||||
displayDig(startDigit,GETHIGHFLASH(_digits,(value & 0x0F)));
|
||||
value>>=4;
|
||||
break;
|
||||
case _DF_RAW:// Raw 7-segment pattern
|
||||
displayDig(startDigit,value & 0xFF);
|
||||
value>>=8;
|
||||
break;
|
||||
default:
|
||||
DIAG(F("TM1368 invalid mode 0x%x"),mode);
|
||||
return;
|
||||
}
|
||||
startDigit++;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t TM1638::getButtons(){
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, LOW);
|
||||
writeData(INSTRUCTION_READ_KEY);
|
||||
pinMode(_dio_pin, INPUT);
|
||||
ArduinoPins::fastWriteDigital(_clk_pin, LOW);
|
||||
uint8_t buttons=0;
|
||||
for (uint8_t eachByte=0; eachByte<4;eachByte++) {
|
||||
uint8_t value = 0;
|
||||
for (uint8_t eachBit = 0; eachBit < 8; eachBit++) {
|
||||
ArduinoPins::fastWriteDigital(_clk_pin, HIGH);
|
||||
value |= ArduinoPins::fastReadDigital(_dio_pin) << eachBit;
|
||||
ArduinoPins::fastWriteDigital(_clk_pin, LOW);
|
||||
}
|
||||
buttons |= value << eachByte;
|
||||
delayMicroseconds(1);
|
||||
}
|
||||
pinMode(_dio_pin, OUTPUT);
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, HIGH);
|
||||
return buttons;
|
||||
}
|
||||
|
||||
|
||||
void TM1638::displayDig(uint8_t digitId, uint8_t pgfedcba){
|
||||
if (digitId>7) return;
|
||||
setDataInstruction(DISPLAY_TURN_ON | _pulse);
|
||||
setDataInstruction(INSTRUCTION_WRITE_DATA| INSTRUCTION_ADDRESS_FIXED);
|
||||
writeDataAt(FIRST_DISPLAY_ADDRESS+14-(digitId*2), pgfedcba);
|
||||
}
|
||||
|
||||
void TM1638::displayClear(){
|
||||
setDataInstruction(DISPLAY_TURN_ON | _pulse);
|
||||
setDataInstruction(INSTRUCTION_WRITE_DATA | INSTRUCTION_ADDRESS_FIXED);
|
||||
for (uint8_t i=0;i<15;i+=2){
|
||||
writeDataAt(FIRST_DISPLAY_ADDRESS+i,0x00);
|
||||
}
|
||||
}
|
||||
|
||||
void TM1638::writeLed(uint8_t num,bool state){
|
||||
if ((num<1) | (num>8)) return;
|
||||
setDataInstruction(DISPLAY_TURN_ON | _pulse);
|
||||
setDataInstruction(INSTRUCTION_WRITE_DATA | INSTRUCTION_ADDRESS_FIXED);
|
||||
writeDataAt(FIRST_DISPLAY_ADDRESS + (num*2-1), state);
|
||||
}
|
||||
|
||||
|
||||
void TM1638::writeData(uint8_t data){
|
||||
for (uint8_t i = 0; i < 8; i++) {
|
||||
ArduinoPins::fastWriteDigital(_dio_pin, data & 1);
|
||||
data >>= 1;
|
||||
ArduinoPins::fastWriteDigital(_clk_pin, HIGH);
|
||||
ArduinoPins::fastWriteDigital(_clk_pin, LOW);
|
||||
}
|
||||
}
|
||||
|
||||
void TM1638::writeDataAt(uint8_t displayAddress, uint8_t data){
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, LOW);
|
||||
writeData(displayAddress);
|
||||
writeData(data);
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, HIGH);
|
||||
delayMicroseconds(1);
|
||||
}
|
||||
|
||||
void TM1638::setDataInstruction(uint8_t dataInstruction){
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, LOW);
|
||||
writeData(dataInstruction);
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, HIGH);
|
||||
delayMicroseconds(1);
|
||||
}
|
||||
|
||||
void TM1638::test(){
|
||||
DIAG(F("TM1638 test"));
|
||||
uint8_t val=0;
|
||||
for(uint8_t i=0;i<5;i++){
|
||||
setDataInstruction(DISPLAY_TURN_ON | _pulse);
|
||||
setDataInstruction(INSTRUCTION_WRITE_DATA| INSTRUCTION_ADDRESS_AUTO);
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, LOW);
|
||||
writeData(FIRST_DISPLAY_ADDRESS);
|
||||
for(uint8_t i=0;i<16;i++)
|
||||
writeData(val);
|
||||
ArduinoPins::fastWriteDigital(_stb_pin, HIGH);
|
||||
delay(1000);
|
||||
val = ~val;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
134
IO_TM1638.h
Normal file
134
IO_TM1638.h
Normal file
@@ -0,0 +1,134 @@
|
||||
/*
|
||||
* © 2024, Chris Harlow. All rights reserved.
|
||||
*
|
||||
* This file is part of DCC++EX API
|
||||
*
|
||||
* 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 IO_TM1638_h
|
||||
#define IO_TM1638_h
|
||||
#include <Arduino.h>
|
||||
#include "IODevice.h"
|
||||
#include "DIAG.h"
|
||||
|
||||
class TM1638 : public IODevice {
|
||||
private:
|
||||
|
||||
uint8_t _buttons;
|
||||
uint8_t _leds;
|
||||
unsigned long _lastLoop;
|
||||
static const int LoopHz=20;
|
||||
|
||||
static const byte
|
||||
INSTRUCTION_WRITE_DATA=0x40,
|
||||
INSTRUCTION_READ_KEY=0x42,
|
||||
INSTRUCTION_ADDRESS_AUTO=0x40,
|
||||
INSTRUCTION_ADDRESS_FIXED=0x44,
|
||||
INSTRUCTION_NORMAL_MODE=0x40,
|
||||
INSTRUCTION_TEST_MODE=0x48,
|
||||
|
||||
FIRST_DISPLAY_ADDRESS=0xC0,
|
||||
|
||||
DISPLAY_TURN_OFF=0x80,
|
||||
DISPLAY_TURN_ON=0x88;
|
||||
|
||||
|
||||
uint8_t _clk_pin;
|
||||
uint8_t _stb_pin;
|
||||
uint8_t _dio_pin;
|
||||
uint8_t _pulse;
|
||||
bool _isOn;
|
||||
|
||||
|
||||
// Constructor
|
||||
TM1638(VPIN firstVpin, byte clk_pin,byte dio_pin,byte stb_pin);
|
||||
|
||||
public:
|
||||
enum DigitFormat : byte {
|
||||
// last 4 bits are length.
|
||||
// DF_1.. DF_8 decimal
|
||||
DF_1=0x01,DF_2=0x02,DF_3=0x03,DF_4=0x04,
|
||||
DF_5=0x05,DF_6=0x06,DF_7=0x07,DF_8=0x08,
|
||||
// DF_1X.. DF_8X HEX
|
||||
DF_1X=0x11,DF_2X=0x12,DF_3X=0x13,DF_4X=0x14,
|
||||
DF_5X=0x15,DF_6X=0x16,DF_7X=0x17,DF_8X=0x18,
|
||||
// DF_1R .. DF_4R raw 7 segmnent data
|
||||
// only 4 because HAL analogWrite only passes 4 bytes
|
||||
DF_1R=0x21,DF_2R=0x22,DF_3R=0x23,DF_4R=0x24,
|
||||
|
||||
// bits of data conversion type (ored with length)
|
||||
_DF_DECIMAL=0x00,// right adjusted decimal unsigned leading zeros
|
||||
_DF_HEX=0x10, // right adjusted hex leading zeros
|
||||
_DF_RAW=0x20 // bytes are raw 7-segment pattern (max length 4)
|
||||
};
|
||||
|
||||
static void create(VPIN firstVpin, byte clk_pin,byte dio_pin,byte stb_pin);
|
||||
|
||||
// Functions overridden in IODevice
|
||||
void _begin();
|
||||
void _loop(unsigned long currentMicros) override ;
|
||||
void _writeAnalogue(VPIN vpin, int value, uint8_t param1, uint16_t param2) override;
|
||||
void _display() override ;
|
||||
int _read(VPIN pin) override;
|
||||
void _write(VPIN pin,int value) override;
|
||||
|
||||
// Device driving functions
|
||||
private:
|
||||
enum pulse_t {
|
||||
PULSE1_16,
|
||||
PULSE2_16,
|
||||
PULSE4_16,
|
||||
PULSE10_16,
|
||||
PULSE11_16,
|
||||
PULSE12_16,
|
||||
PULSE13_16,
|
||||
PULSE14_16
|
||||
};
|
||||
|
||||
/**
|
||||
* @fn getButtons
|
||||
* @return state of 8 buttons
|
||||
*/
|
||||
uint8_t getButtons();
|
||||
|
||||
/**
|
||||
* @fn writeLed
|
||||
* @brief put led ON or OFF
|
||||
* @param num num of led(1-8)
|
||||
* @param state (true or false)
|
||||
*/
|
||||
void writeLed(uint8_t num, bool state);
|
||||
|
||||
|
||||
/**
|
||||
* @fn displayDig
|
||||
* @brief set 7 segment display + dot
|
||||
* @param digitId num of digit(0-7)
|
||||
* @param val value 8 bits
|
||||
*/
|
||||
void displayDig(uint8_t digitId, uint8_t pgfedcba);
|
||||
|
||||
/**
|
||||
* @fn displayClear
|
||||
* @brief switch off all leds and segment display
|
||||
*/
|
||||
void displayClear();
|
||||
void test();
|
||||
void writeData(uint8_t data);
|
||||
void writeDataAt(uint8_t displayAddress, uint8_t data);
|
||||
void setDisplayMode(uint8_t displayMode);
|
||||
void setDataInstruction(uint8_t dataInstruction);
|
||||
};
|
||||
#endif
|
@@ -26,7 +26,7 @@
|
||||
Thus "MAIN"_hk generates exactly the same run time vakue
|
||||
as const int16_t HASH_KEYWORD_MAIN=11339
|
||||
*/
|
||||
#ifndef KeywordHAsher_h
|
||||
#ifndef KeywordHasher_h
|
||||
#define KeywordHasher_h
|
||||
|
||||
#include <Arduino.h>
|
||||
@@ -54,4 +54,43 @@ static_assert("MAIN"_hk == 11339,"Keyword hasher error");
|
||||
static_assert("SLOW"_hk == -17209,"Keyword hasher error");
|
||||
static_assert("SPEED28"_hk == -17064,"Keyword hasher error");
|
||||
static_assert("SPEED128"_hk == 25816,"Keyword hasher error");
|
||||
|
||||
// Compile time converter from "abcd"_s7 to the 7 segment nearest equivalent
|
||||
|
||||
constexpr uint8_t seg7Digits[]={
|
||||
0b00111111,0b00000110,0b01011011,0b01001111, // 0..3
|
||||
0b01100110,0b01101101,0b01111101,0b00000111, // 4..7
|
||||
0b01111111,0b01101111 // 8..9
|
||||
};
|
||||
|
||||
constexpr uint8_t seg7Letters[]={
|
||||
0b01110111,0b01111100,0b00111001,0b01011110, // ABCD
|
||||
0b01111001,0b01110001,0b00111101,0b01110110, // EFGH
|
||||
0b00000100,0b00011110,0b01110010,0b00111000, //IJKL
|
||||
0b01010101,0b01010100,0b01011100,0b01110011, // MNOP
|
||||
0b10111111,0b01010000,0b01101101,0b01111000, // QRST
|
||||
0b00111110,0b00011100,0b01101010,0b01001001, //UVWX
|
||||
0b01100110,0b01011011 //YZ
|
||||
};
|
||||
constexpr uint8_t seg7Space=0b00000000;
|
||||
constexpr uint8_t seg7Minus=0b01000000;
|
||||
constexpr uint8_t seg7Equals=0b01001000;
|
||||
|
||||
|
||||
constexpr uint32_t CompiletimeSeg7(const char * sv, uint32_t running, size_t rlen) {
|
||||
return (*sv==0 || rlen==0) ? running << (8*rlen) : CompiletimeSeg7(sv+1,
|
||||
(*sv >= '0' && *sv <= '9') ? (running<<8) | seg7Digits[*sv-'0'] :
|
||||
(*sv >= 'A' && *sv <= 'Z') ? (running<<8) | seg7Letters[*sv-'A'] :
|
||||
(*sv >= 'a' && *sv <= 'z') ? (running<<8) | seg7Letters[*sv-'a'] :
|
||||
(*sv == '-') ? (running<<8) | seg7Minus :
|
||||
(*sv == '=') ? (running<<8) | seg7Equals :
|
||||
(running<<8) | seg7Space,
|
||||
rlen-1
|
||||
); //
|
||||
}
|
||||
|
||||
constexpr uint32_t operator""_s7(const char * keyword, size_t len)
|
||||
{
|
||||
return CompiletimeSeg7(keyword,0*len,4);
|
||||
}
|
||||
#endif
|
@@ -1,5 +1,6 @@
|
||||
/*
|
||||
* © 2022-2024 Paul M Antoine
|
||||
* © 2024 Herb Morton
|
||||
* © 2021 Mike S
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2023 Harald Barth
|
||||
@@ -98,7 +99,7 @@ MotorDriver::MotorDriver(int16_t power_pin, byte signal_pin, byte signal_pin2, i
|
||||
if (HAVE_PORTH(fastSignalPin.inout == &PORTH)) {
|
||||
DIAG(F("Found PORTH pin %d"),signalPin);
|
||||
fastSignalPin.shadowinout = fastSignalPin.inout;
|
||||
fastSignalPin.inout = &shadowPORTF;
|
||||
fastSignalPin.inout = &shadowPORTH;
|
||||
}
|
||||
|
||||
signalPin2=signal_pin2;
|
||||
@@ -575,7 +576,7 @@ void MotorDriver::checkPowerOverload(bool useProgLimit, byte trackno) {
|
||||
DIAG(F("TRACK %c ALERT FAULT"), trackno + 'A');
|
||||
}
|
||||
setPower(POWERMODE::ALERT);
|
||||
if ((trackMode & TRACK_MODE_AUTOINV) && (trackMode & (TRACK_MODE_MAIN|TRACK_MODE_EXT|TRACK_MODE_BOOST))){
|
||||
if ((trackMode & TRACK_MODIFIER_AUTO) && (trackMode & (TRACK_MODE_MAIN|TRACK_MODE_EXT|TRACK_MODE_BOOST))){
|
||||
DIAG(F("TRACK %c INVERT"), trackno + 'A');
|
||||
invertOutput();
|
||||
}
|
||||
@@ -638,6 +639,10 @@ void MotorDriver::checkPowerOverload(bool useProgLimit, byte trackno) {
|
||||
}
|
||||
throttleInrush(false);
|
||||
setPower(POWERMODE::ON);
|
||||
break;
|
||||
}
|
||||
if (goodtime > POWER_SAMPLE_ALERT_GOOD/2) {
|
||||
throttleInrush(false);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
@@ -29,21 +29,33 @@
|
||||
#include <wiring_private.h>
|
||||
|
||||
// use powers of two so we can do logical and/or on the track modes in if clauses.
|
||||
// RACK_MODE_DCX is (TRACK_MODE_DC|TRACK_MODE_INV)
|
||||
// For example TRACK_MODE_DC_INV is (TRACK_MODE_DC|TRACK_MODIFIER_INV)
|
||||
template<class T> inline T operator~ (T a) { return (T)~(int)a; }
|
||||
template<class T> inline T operator| (T a, T b) { return (T)((int)a | (int)b); }
|
||||
template<class T> inline T operator& (T a, T b) { return (T)((int)a & (int)b); }
|
||||
template<class T> inline T operator^ (T a, T b) { return (T)((int)a ^ (int)b); }
|
||||
enum TRACK_MODE : byte {TRACK_MODE_NONE = 1, TRACK_MODE_MAIN = 2, TRACK_MODE_PROG = 4,
|
||||
TRACK_MODE_DC = 8, TRACK_MODE_EXT = 16,
|
||||
enum TRACK_MODE : byte {
|
||||
// main modes
|
||||
TRACK_MODE_NONE = 1, TRACK_MODE_MAIN = 2, TRACK_MODE_PROG = 4,
|
||||
TRACK_MODE_DC = 8, TRACK_MODE_EXT = 16,
|
||||
// modifiers
|
||||
TRACK_MODIFIER_INV = 64, TRACK_MODIFIER_AUTO = 128,
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
TRACK_MODE_BOOST = 32,
|
||||
TRACK_MODE_BOOST = 32,
|
||||
TRACK_MODE_BOOST_INV = TRACK_MODE_BOOST|TRACK_MODIFIER_INV,
|
||||
TRACK_MODE_BOOST_AUTO = TRACK_MODE_BOOST|TRACK_MODIFIER_AUTO,
|
||||
#else
|
||||
TRACK_MODE_BOOST = 0,
|
||||
TRACK_MODE_BOOST = 0,
|
||||
TRACK_MODE_BOOST_INV = 0,
|
||||
TRACK_MODE_BOOST_AUTO = 0,
|
||||
#endif
|
||||
TRACK_MODE_ALL = TRACK_MODE_MAIN|TRACK_MODE_PROG|TRACK_MODE_DC|TRACK_MODE_EXT|TRACK_MODE_BOOST,
|
||||
TRACK_MODE_INV = 64,
|
||||
TRACK_MODE_DCX = TRACK_MODE_DC|TRACK_MODE_INV, TRACK_MODE_AUTOINV = 128};
|
||||
// derived modes; TRACK_ALL is calles that so it does not match TRACK_MODE_*
|
||||
TRACK_ALL = TRACK_MODE_MAIN|TRACK_MODE_PROG|TRACK_MODE_DC|TRACK_MODE_EXT|TRACK_MODE_BOOST,
|
||||
TRACK_MODE_MAIN_INV = TRACK_MODE_MAIN|TRACK_MODIFIER_INV,
|
||||
TRACK_MODE_MAIN_AUTO = TRACK_MODE_MAIN|TRACK_MODIFIER_AUTO,
|
||||
TRACK_MODE_DC_INV = TRACK_MODE_DC|TRACK_MODIFIER_INV,
|
||||
TRACK_MODE_DCX = TRACK_MODE_DC_INV // DCX is other name for historical reasons
|
||||
};
|
||||
|
||||
#define setHIGH(fastpin) *fastpin.inout |= fastpin.maskHIGH
|
||||
#define setLOW(fastpin) *fastpin.inout &= fastpin.maskLOW
|
||||
@@ -273,7 +285,7 @@ class MotorDriver {
|
||||
#endif
|
||||
inline void setMode(TRACK_MODE m) {
|
||||
trackMode = m;
|
||||
invertOutput(trackMode & TRACK_MODE_INV);
|
||||
invertOutput(trackMode & TRACK_MODIFIER_INV);
|
||||
};
|
||||
inline void invertOutput() { // toggles output inversion
|
||||
invertPhase = !invertPhase;
|
||||
|
@@ -75,11 +75,19 @@
|
||||
#define SAMD_STANDARD_MOTOR_SHIELD STANDARD_MOTOR_SHIELD
|
||||
#define STM32_STANDARD_MOTOR_SHIELD STANDARD_MOTOR_SHIELD
|
||||
|
||||
#if defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F439ZI) || defined(ARDUINO_NUCLEO_F4X9ZI)
|
||||
// EX 8874 based shield connected to a 3V3 system with 12-bit (4096) ADC
|
||||
// The Ethernet capable STM32 models cannot use Channel B BRAKE on D8, and must use the ALT pin of D6,
|
||||
// AND cannot use Channel B PWN on D11, but must use the ALT pin of D5
|
||||
#define EX8874_SHIELD F("EX8874"), \
|
||||
new MotorDriver( 3, 12, UNUSED_PIN, 9, A0, 1.27, 5000, A4), \
|
||||
new MotorDriver( 5, 13, UNUSED_PIN, 6, A1, 1.27, 5000, A5)
|
||||
#else
|
||||
// EX 8874 based shield connected to a 3V3 system with 12-bit (4096) ADC
|
||||
#define EX8874_SHIELD F("EX8874"), \
|
||||
new MotorDriver( 3, 12, UNUSED_PIN, 9, A0, 1.27, 5000, A4), \
|
||||
new MotorDriver(11, 13, UNUSED_PIN, 8, A1, 1.27, 5000, A5)
|
||||
|
||||
#endif
|
||||
|
||||
#elif defined(ARDUINO_ARCH_ESP32)
|
||||
// STANDARD shield on an ESPDUINO-32 (ESP32 in Uno form factor). The shield must be eiter the
|
||||
@@ -97,6 +105,18 @@
|
||||
new MotorDriver(25/* 3*/, 19/*12*/, UNUSED_PIN, 13/*9*/, 35/*A2*/, 1.27, 5000, 36 /*A4*/), \
|
||||
new MotorDriver(23/*11*/, 18/*13*/, UNUSED_PIN, 12/*8*/, 34/*A3*/, 1.27, 5000, 39 /*A5*/)
|
||||
|
||||
// EX-CSB1 with integrated motor driver definition
|
||||
#define EXCSB1 F("EXCSB1"),\
|
||||
new MotorDriver(25, 0, UNUSED_PIN, -14, 34, 2.23, 5000, 19), \
|
||||
new MotorDriver(27, 15, UNUSED_PIN, -2, 35, 2.23, 5000, 23)
|
||||
|
||||
// EX-CSB1 with EX-8874 stacked on top for 4 outputs
|
||||
#define EXCSB1_WITH_EX8874 F("EXCSB1_WITH_EX8874"),\
|
||||
new MotorDriver(25, 0, UNUSED_PIN, -14, 34, 2.23, 5000, 19), \
|
||||
new MotorDriver(27, 15, UNUSED_PIN, -2, 35, 2.23, 5000, 23), \
|
||||
new MotorDriver(26, 5, UNUSED_PIN, 13, 36, 1.52, 5000, 18), \
|
||||
new MotorDriver(16, 4, UNUSED_PIN, 12, 39, 1.52, 5000, 17)
|
||||
|
||||
#else
|
||||
// STANDARD shield on any Arduino Uno or Mega compatible with the original specification.
|
||||
#define STANDARD_MOTOR_SHIELD F("STANDARD_MOTOR_SHIELD"), \
|
||||
|
77
Release_Notes/NeoPixel.md
Normal file
77
Release_Notes/NeoPixel.md
Normal file
@@ -0,0 +1,77 @@
|
||||
NeoPixel support
|
||||
|
||||
The IO_NeoPixel.h driver supports the adafruit neopixel seesaw board. It turns each pixel into an individual VPIN which can be given a colour and turned on or off using the new <o> command or the NEOPIXEL Exrail macro. Exrail SIGNALS can also drive a single pixel signal or multiple separate pixels.
|
||||
|
||||
|
||||
1. Defining the hardware driver:
|
||||
Add a driver definition in myAutomation.h for each adafruit I2C driver.
|
||||
|
||||
HAL(neoPixel, firstVpin, numberOfPixels [, mode [, i2caddress])
|
||||
Where mode is selected from the various pixel string types which have varying
|
||||
colour order or refresh frequency. For MOST strings this mode will be NEO_GRB but for others refer to the comments in IO_NeoPixel.h
|
||||
If omitted the node and i2caddress default to NEO_GRB, 0x60.
|
||||
|
||||
HAL(NeoPixel,1000,20)
|
||||
This is a NeoPixel driver defaulting to I2C aqddress 0x60 for a GRB pixel string. Pixels are given vpin numbers from 1000 to 1019.
|
||||
HAL(NeoPixel,1020,20,NEO_GRB,0x61)
|
||||
This is a NeoPixel driver on i2c address 0x61
|
||||
|
||||
2. Setting pixels from the < > commands.
|
||||
By default, each pixel in the string is created as white but switched off.
|
||||
Each pixel has a vpin starting from the first vpin in the HAL definitions.
|
||||
|
||||
<o vpin> switches pixel on (same as <z vpin>) e.g. <o 1005>
|
||||
<o -vpin> switches pixel off (same as <z -vpin>) e.g. <o -1003>
|
||||
(the z commands work on pixels the same as other gpio pins.)
|
||||
|
||||
<o [-]vpin count> switches on/off count pixels starting at vpin. e.g <o 1000 5>
|
||||
Note: it IS acceptable to switch across 2 strings of pixels if they are contiguous vpin ranges. It is also interesting that this command doesnt care if the vpins are NeoPixel or any other type, so it can be used to switch a range of other pin types.
|
||||
|
||||
<o [-]vpin red green blue [count]> sets the colour and on/off status of a pin or pins. Each colour is 0..255 e.g. <o 1005 255 255 0> sets pin 1005 to bright yellow and ON, <0 -1006 0 0 255 10> sets pins 1006 to 1015 (10 pins) to bright blue but OFF.
|
||||
Note: If you set a pin to a colour, you can turn it on and off without having to reset the colour every time. This is something the adafruit seesaw library can't do and is just one of several reasons why we dont use it.
|
||||
|
||||
3. Setting pixels from EXRAIL
|
||||
The new NEOPIXEL macro provides the same functionality as the <o [-]vpin red green blue [count]> command above.
|
||||
NEOPIXEL([-]vpin, red, green, blue [,count])
|
||||
|
||||
Setting pixels on or off (without colour change) can be done with SET/RESET [currently there is no set range facility but that may be added as a general exrail thing... watch this space]
|
||||
|
||||
Because the pixels obey set/reset, the BLINK command can also be used to control blinking a pixel.
|
||||
|
||||
4. EXRAIL pixel signals.
|
||||
There are two types possible, a mast with separate fixed colour pixels for each aspect, or a mast with one multiple colour pixel for all aspects.
|
||||
|
||||
For separate pixels, the colours should be established at startup and a normal SIGNALH macro used.
|
||||
|
||||
AUTOSTART
|
||||
SIGNALH(1010,1011,1012)
|
||||
NEOPIXEL(1010,255,0,0)
|
||||
NEOPIXEL(1011,128,128,0)
|
||||
NEOPIXEL(1012,0,255,0)
|
||||
RED(1010) // force signal state otherwise all 3 lights will be on
|
||||
DONE
|
||||
|
||||
For signals with 1 pixel, the NEOPIXEL_SIGNAL macro will create a signal
|
||||
NEOPIXEL_SIGNAL(vpin,redfx,amberfx,greenfx)
|
||||
|
||||
** Changed... ****
|
||||
The fx values above can be created by the NeoRGB macro so a bright red would be NeoRGB(255,0,0) bright green NeoRGB(0,255,0) and amber something like NeoRGB(255,100,0)
|
||||
NeoRGB creates a single int32_t value so it can be used in several ways as convenient.
|
||||
|
||||
// create 1-lamp signal with NeoRGB colours
|
||||
NEOPIXEL_SIGNAL(1000,NeoRGB(255,0,0),NeoRGB(255,100,0),NeoRGB(0,255,0))
|
||||
|
||||
// Create 1-lamp signal with named colours.
|
||||
// This is better if you have multiple signals.
|
||||
// (Note: ALIAS is not suitable due to word length defaults)
|
||||
#define REDLAMP NeoRGB(255,0,0)
|
||||
#define AMBERLAMP NeoRGB(255,100,0)
|
||||
#define GREENLAMP NeoRGB(0,255,0)
|
||||
NEOPIXEL_SIGNAL(1001,REDLAMP,AMBERLAMP,GREENLAMP)
|
||||
|
||||
// Create 1-lamp signal with web type RGB colours
|
||||
// (Using blue for the amber signal , just testing)
|
||||
NEOPIXEL_SIGNAL(1002,0xFF0000,0x0000FF,0x00FF00)
|
||||
|
||||
|
||||
|
84
Release_Notes/TM1638.md
Normal file
84
Release_Notes/TM1638.md
Normal file
@@ -0,0 +1,84 @@
|
||||
## TM1638 ##
|
||||
|
||||
The TM1638 board provides a very cheap way of implementing 8 buttons, 8 leds and an 8 digit 7segment display in a package requiring just 5 Dupont wires (vcc, gnd + 3 GPIO pins) from the command station without soldering.
|
||||
|
||||
|
||||
This is ideal for prototyping and testing, simulating sensors and signals, displaying states etc. For a built layout, this could provide a control for things that are not particularly suited to throttle 'route' buttons, perhaps lineside automations or fiddle yard lane selection.
|
||||
|
||||
By adding a simple HAL statement to myAutomation.h it creates 8 buttons/sensors and 8 leds.
|
||||
|
||||
`HAL(TM1638,500,29,31,33)`
|
||||
Creates VPINs 500-507 And desscribes the GPIO pins used to connect the clk,dio,stb pins on the TM1638 board.
|
||||
|
||||
Setting each of the VPINs will control the associated LED (using for example SET, RESET or BLINK in Exrail or `<z 500> <z -501> from a command).
|
||||
|
||||
Unlike most pins, you can also read the same pin number and get the button state, using Exrail IF/AT/ONBUTTON etc.
|
||||
|
||||
For example:
|
||||
`
|
||||
HAL(TM1638,500,29,31,33)
|
||||
`
|
||||
All the folowing examples assume you are using VPIN 500 as the first, leftmost, led/button on the TM1638 board.
|
||||
|
||||
|
||||
`ONBUTTON(500)
|
||||
SET(500) // light the first led
|
||||
BLINK(501,500,500) // blink the second led
|
||||
SETLOCO(3) FWD(50) // set a loco going
|
||||
AT(501) STOP // press second button to stop
|
||||
RESET(500) RESET(501) // turn leds off
|
||||
DONE
|
||||
`
|
||||
|
||||
Buttons behave like any other sensor, so using `<S 500 500 1>` will cause the command station to issue `<Q 500>` and `<q 500>` messages when the first button is pressed or released.
|
||||
|
||||
Exrail `JMRI_SENSOR(500,8)` will create `<S` commands for all 8 buttons.
|
||||
|
||||
## Using the 7 Segment display ##
|
||||
|
||||
The 8 digit display can be treated as 8 separate digits (left most being the same VPIN as the leftmost button and led) or be written to in sections of any length. Writing uses the existing analogue interface to the common HAL but is awkward to use directly. To make this easier from Exrail, a SEG7 macro provides a remapping to the ANOUT facility that makes more sense.
|
||||
|
||||
SEG7(vpin,value,format)
|
||||
|
||||
The vpin determins which digit to start writing at.
|
||||
The value can be a 32bit unsigned integer but is interpreted differentlky according to the format.
|
||||
|
||||
Format values:
|
||||
1..8 give the length (number of display digits) to fill, and defaults to decimal number with leading zeros.
|
||||
|
||||
1X..8X give the length but display in hex.
|
||||
|
||||
1R..4R treats each byte of the value as raw 7-segment patterns so that it can write letters and symbols using any compination of the 7segments and deciml point.
|
||||
|
||||
There is a useful description here:
|
||||
https://jetpackacademy.com/wp-content/uploads/2018/06/TM1638_cheat_sheet_download.pdf
|
||||
|
||||
|
||||
e.g. SEG7(500,3,4)
|
||||
writes 0003 to first 4 digits of the display
|
||||
SEG7(504,0xcafe,4X)
|
||||
writes CAFE to the last 4 digits
|
||||
SEG7(500,0xdeadbeef,8X)
|
||||
writes dEAdbEEF to all 8 digits.
|
||||
|
||||
Writing raw segment patters requires knowledge of the bit pattern to segment relationship:
|
||||
` 0
|
||||
== 0 ==
|
||||
5| | 1
|
||||
== 6 ==
|
||||
4 | | 2
|
||||
== 3 ==
|
||||
7=decimal point
|
||||
|
||||
Thus Letter A is segments 6 5 4 2 1 0, in bits that is (0 bit on right)
|
||||
0b01110111 or 0x77
|
||||
This is not easy to do my hand and thus a new string type suffix has been introduced to make simple text messages. Note that the HAL interface only has width for 32 bits which is only 4 symbols so writing 8 digits requires two calls.
|
||||
|
||||
e.g. SEG7(500,"Hell"_s7,4R) SEG7(504,"o"_s7,4R)
|
||||
DELAY(1000)
|
||||
SEG7(500,"Worl"_s7,4R) SEG7(504,"d"_s7,4R)
|
||||
|
||||
Note that some letters like k,m,v,x do not have particularly readable 7-segment representations.
|
||||
|
||||
Credit to https://github.com/dvarrel/TM1638 for the basic formulae.
|
||||
|
@@ -1,7 +1,7 @@
|
||||
/*
|
||||
* © 2022 Paul M. Antoine
|
||||
* © 2021 Chris Harlow
|
||||
* © 2022 Harald Barth
|
||||
* © 2022 2024 Harald Barth
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of DCC++EX
|
||||
@@ -23,6 +23,7 @@
|
||||
#include "SerialManager.h"
|
||||
#include "DCCEXParser.h"
|
||||
#include "StringFormatter.h"
|
||||
#include "DIAG.h"
|
||||
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
#ifdef SERIAL_BT_COMMANDS
|
||||
@@ -36,6 +37,10 @@ BluetoothSerial SerialBT;
|
||||
#endif //COMMANDS
|
||||
#endif //ESP32
|
||||
|
||||
static const byte PAYLOAD_FALSE = 0;
|
||||
static const byte PAYLOAD_NORMAL = 1;
|
||||
static const byte PAYLOAD_STRING = 2;
|
||||
|
||||
SerialManager * SerialManager::first=NULL;
|
||||
|
||||
SerialManager::SerialManager(Stream * myserial) {
|
||||
@@ -43,7 +48,7 @@ SerialManager::SerialManager(Stream * myserial) {
|
||||
next=first;
|
||||
first=this;
|
||||
bufferLength=0;
|
||||
inCommandPayload=false;
|
||||
inCommandPayload=PAYLOAD_FALSE;
|
||||
}
|
||||
|
||||
void SerialManager::init() {
|
||||
@@ -68,7 +73,11 @@ void SerialManager::init() {
|
||||
new SerialManager(&Serial3);
|
||||
#endif
|
||||
#ifdef SERIAL2_COMMANDS
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
Serial2.begin(115200, SERIAL_8N1, 16, 17); // GPIO 16 RXD2; GPIO 17 TXD2 on ESP32
|
||||
#else // not ESP32
|
||||
Serial2.begin(115200);
|
||||
#endif // ESP32
|
||||
new SerialManager(&Serial2);
|
||||
#endif
|
||||
#ifdef SERIAL1_COMMANDS
|
||||
@@ -88,7 +97,11 @@ void SerialManager::init() {
|
||||
}
|
||||
#endif
|
||||
#ifdef SABERTOOTH
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
Serial2.begin(9600, SERIAL_8N1, 16, 17); // GPIO 16 RXD2; GPIO 17 TXD2 on ESP32
|
||||
#else
|
||||
Serial2.begin(9600);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -104,23 +117,39 @@ void SerialManager::loop() {
|
||||
}
|
||||
|
||||
void SerialManager::loop2() {
|
||||
while (serial->available()) {
|
||||
char ch = serial->read();
|
||||
if (ch == '<') {
|
||||
inCommandPayload = true;
|
||||
bufferLength = 0;
|
||||
buffer[0] = '\0';
|
||||
}
|
||||
else if (inCommandPayload) {
|
||||
if (bufferLength < (COMMAND_BUFFER_SIZE-1))
|
||||
buffer[bufferLength++] = ch;
|
||||
if (ch == '>') {
|
||||
buffer[bufferLength] = '\0';
|
||||
DCCEXParser::parse(serial, buffer, NULL);
|
||||
inCommandPayload = false;
|
||||
break;
|
||||
}
|
||||
while (serial->available()) {
|
||||
char ch = serial->read();
|
||||
if (!inCommandPayload) {
|
||||
if (ch == '<') {
|
||||
inCommandPayload = PAYLOAD_NORMAL;
|
||||
bufferLength = 0;
|
||||
buffer[0] = '\0';
|
||||
}
|
||||
} else { // if (inCommandPayload)
|
||||
if (bufferLength < (COMMAND_BUFFER_SIZE-1))
|
||||
buffer[bufferLength++] = ch;
|
||||
if (inCommandPayload > PAYLOAD_NORMAL) {
|
||||
if (inCommandPayload > 32 + 2) { // String way too long
|
||||
ch = '>'; // we end this nonsense
|
||||
inCommandPayload = PAYLOAD_NORMAL;
|
||||
DIAG(F("Parse error: Unbalanced string"));
|
||||
// fall through to ending parsing below
|
||||
} else if (ch == '"') { // String end
|
||||
inCommandPayload = PAYLOAD_NORMAL;
|
||||
continue; // do not fall through
|
||||
} else
|
||||
inCommandPayload++;
|
||||
}
|
||||
if (inCommandPayload == PAYLOAD_NORMAL) {
|
||||
if (ch == '>') {
|
||||
buffer[bufferLength] = '\0';
|
||||
DCCEXParser::parse(serial, buffer, NULL);
|
||||
inCommandPayload = PAYLOAD_FALSE;
|
||||
break;
|
||||
} else if (ch == '"') {
|
||||
inCommandPayload = PAYLOAD_STRING;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
@@ -44,6 +44,6 @@ private:
|
||||
SerialManager * next;
|
||||
byte bufferLength;
|
||||
byte buffer[COMMAND_BUFFER_SIZE];
|
||||
bool inCommandPayload;
|
||||
byte inCommandPayload;
|
||||
};
|
||||
#endif
|
||||
|
@@ -139,6 +139,7 @@ void StringFormatter::send2(Print * stream,const FSH* format, va_list args) {
|
||||
case 'd': printPadded(stream,va_arg(args, int), formatWidth, formatLeft); break;
|
||||
case 'u': printPadded(stream,va_arg(args, unsigned int), formatWidth, formatLeft); break;
|
||||
case 'l': printPadded(stream,va_arg(args, long), formatWidth, formatLeft); break;
|
||||
case 'L': stream->print(va_arg(args, unsigned long), DEC); break;
|
||||
case 'b': stream->print(va_arg(args, int), BIN); break;
|
||||
case 'o': stream->print(va_arg(args, int), OCT); break;
|
||||
case 'x': stream->print((unsigned int)va_arg(args, unsigned int), HEX); break;
|
||||
|
@@ -1,6 +1,8 @@
|
||||
/*
|
||||
* © 2022 Chris Harlow
|
||||
* © 2022-2024 Harald Barth
|
||||
* © 2023-2024 Paul M. Antoine
|
||||
* © 2024 Herb Morton
|
||||
* © 2023 Colin Murdoch
|
||||
* All rights reserved.
|
||||
*
|
||||
@@ -35,7 +37,7 @@
|
||||
|
||||
#define APPLY_BY_MODE(findmode,function) \
|
||||
FOR_EACH_TRACK(t) \
|
||||
if (track[t]->getMode()==findmode) \
|
||||
if (track[t]->getMode() & findmode) \
|
||||
track[t]->function;
|
||||
|
||||
MotorDriver * TrackManager::track[MAX_TRACKS] = { NULL };
|
||||
@@ -149,6 +151,8 @@ void TrackManager::setDCCSignal( bool on) {
|
||||
HAVE_PORTD(shadowPORTD=PORTD);
|
||||
HAVE_PORTE(shadowPORTE=PORTE);
|
||||
HAVE_PORTF(shadowPORTF=PORTF);
|
||||
HAVE_PORTG(shadowPORTG=PORTG);
|
||||
HAVE_PORTH(shadowPORTH=PORTH);
|
||||
APPLY_BY_MODE(TRACK_MODE_MAIN,setSignal(on));
|
||||
HAVE_PORTA(PORTA=shadowPORTA);
|
||||
HAVE_PORTB(PORTB=shadowPORTB);
|
||||
@@ -156,6 +160,8 @@ void TrackManager::setDCCSignal( bool on) {
|
||||
HAVE_PORTD(PORTD=shadowPORTD);
|
||||
HAVE_PORTE(PORTE=shadowPORTE);
|
||||
HAVE_PORTF(PORTF=shadowPORTF);
|
||||
HAVE_PORTG(PORTG=shadowPORTG);
|
||||
HAVE_PORTH(PORTH=shadowPORTH);
|
||||
}
|
||||
|
||||
// setPROGSignal(), called from interrupt context
|
||||
@@ -167,6 +173,8 @@ void TrackManager::setPROGSignal( bool on) {
|
||||
HAVE_PORTD(shadowPORTD=PORTD);
|
||||
HAVE_PORTE(shadowPORTE=PORTE);
|
||||
HAVE_PORTF(shadowPORTF=PORTF);
|
||||
HAVE_PORTG(shadowPORTG=PORTG);
|
||||
HAVE_PORTH(shadowPORTH=PORTH);
|
||||
APPLY_BY_MODE(TRACK_MODE_PROG,setSignal(on));
|
||||
HAVE_PORTA(PORTA=shadowPORTA);
|
||||
HAVE_PORTB(PORTB=shadowPORTB);
|
||||
@@ -174,6 +182,8 @@ void TrackManager::setPROGSignal( bool on) {
|
||||
HAVE_PORTD(PORTD=shadowPORTD);
|
||||
HAVE_PORTE(PORTE=shadowPORTE);
|
||||
HAVE_PORTF(PORTF=shadowPORTF);
|
||||
HAVE_PORTG(PORTG=shadowPORTG);
|
||||
HAVE_PORTH(PORTH=shadowPORTH);
|
||||
}
|
||||
|
||||
// setDCSignal(), called from normal context
|
||||
@@ -377,11 +387,15 @@ bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])
|
||||
if (params>1 && (p[0]<0 || p[0]>=MAX_TRACKS))
|
||||
return false;
|
||||
|
||||
if (params==2 && p[1]=="MAIN"_hk) // <= id MAIN>
|
||||
if (params==2 && p[1]=="MAIN"_hk) // <= id MAIN>
|
||||
return setTrackMode(p[0],TRACK_MODE_MAIN);
|
||||
if (params==2 && p[1]=="MAIN_INV"_hk) // <= id MAIN_INV>
|
||||
return setTrackMode(p[0],TRACK_MODE_MAIN_INV);
|
||||
if (params==2 && p[1]=="MAIN_AUTO"_hk) // <= id MAIN_AUTO>
|
||||
return setTrackMode(p[0],TRACK_MODE_MAIN_AUTO);
|
||||
|
||||
#ifndef DISABLE_PROG
|
||||
if (params==2 && p[1]=="PROG"_hk) // <= id PROG>
|
||||
if (params==2 && p[1]=="PROG"_hk) // <= id PROG>
|
||||
return setTrackMode(p[0],TRACK_MODE_PROG);
|
||||
#endif
|
||||
|
||||
@@ -392,20 +406,27 @@ bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])
|
||||
return setTrackMode(p[0],TRACK_MODE_EXT);
|
||||
#ifdef BOOSTER_INPUT
|
||||
if (TRACK_MODE_BOOST != 0 && // compile time optimization
|
||||
params==2 && p[1]=="BOOST"_hk) // <= id BOOST>
|
||||
params==2 && p[1]=="BOOST"_hk) // <= id BOOST>
|
||||
return setTrackMode(p[0],TRACK_MODE_BOOST);
|
||||
if (TRACK_MODE_BOOST_INV != 0 && // compile time optimization
|
||||
params==2 && p[1]=="BOOST_INV"_hk) // <= id BOOST_INV>
|
||||
return setTrackMode(p[0],TRACK_MODE_BOOST_INV);
|
||||
if (TRACK_MODE_BOOST_AUTO != 0 && // compile time optimization
|
||||
params==2 && p[1]=="BOOST_AUTO"_hk) // <= id BOOST_AUTO>
|
||||
return setTrackMode(p[0],TRACK_MODE_BOOST_AUTO);
|
||||
#endif
|
||||
if (params==2 && p[1]=="AUTO"_hk) // <= id AUTO>
|
||||
return setTrackMode(p[0], track[p[0]]->getMode() | TRACK_MODE_AUTOINV);
|
||||
if (params==2 && p[1]=="AUTO"_hk) // <= id AUTO>
|
||||
return setTrackMode(p[0], track[p[0]]->getMode() | TRACK_MODIFIER_AUTO);
|
||||
|
||||
if (params==2 && p[1]=="INV"_hk) // <= id AUTO>
|
||||
return setTrackMode(p[0], track[p[0]]->getMode() | TRACK_MODE_INV);
|
||||
if (params==2 && p[1]=="INV"_hk) // <= id INV>
|
||||
return setTrackMode(p[0], track[p[0]]->getMode() | TRACK_MODIFIER_INV);
|
||||
|
||||
if (params==3 && p[1]=="DC"_hk && p[2]>0) // <= id DC cab>
|
||||
if (params==3 && p[1]=="DC"_hk && p[2]>0) // <= id DC cab>
|
||||
return setTrackMode(p[0],TRACK_MODE_DC,p[2]);
|
||||
|
||||
if (params==3 && p[1]=="DCX"_hk && p[2]>0) // <= id DCX cab>
|
||||
return setTrackMode(p[0],TRACK_MODE_DC|TRACK_MODE_INV,p[2]);
|
||||
if (params==3 && (p[1]=="DC_INV"_hk || // <= id DC_INV cab>
|
||||
p[1]=="DCX"_hk) && p[2]>0) // <= id DCX cab>
|
||||
return setTrackMode(p[0],TRACK_MODE_DC_INV,p[2]);
|
||||
|
||||
return false;
|
||||
}
|
||||
@@ -414,9 +435,9 @@ const FSH* TrackManager::getModeName(TRACK_MODE tm) {
|
||||
const FSH *modename=F("---");
|
||||
|
||||
if (tm & TRACK_MODE_MAIN) {
|
||||
if(tm & TRACK_MODE_AUTOINV)
|
||||
if(tm & TRACK_MODIFIER_AUTO)
|
||||
modename=F("MAIN A");
|
||||
else if (tm & TRACK_MODE_INV)
|
||||
else if (tm & TRACK_MODIFIER_INV)
|
||||
modename=F("MAIN I>\n");
|
||||
else
|
||||
modename=F("MAIN");
|
||||
@@ -430,15 +451,15 @@ const FSH* TrackManager::getModeName(TRACK_MODE tm) {
|
||||
else if(tm & TRACK_MODE_EXT)
|
||||
modename=F("EXT");
|
||||
else if(tm & TRACK_MODE_BOOST) {
|
||||
if(tm & TRACK_MODE_AUTOINV)
|
||||
if(tm & TRACK_MODIFIER_AUTO)
|
||||
modename=F("BOOST A");
|
||||
else if (tm & TRACK_MODE_INV)
|
||||
else if (tm & TRACK_MODIFIER_INV)
|
||||
modename=F("BOOST I");
|
||||
else
|
||||
modename=F("BOOST");
|
||||
}
|
||||
else if (tm & TRACK_MODE_DC) {
|
||||
if (tm & TRACK_MODE_INV)
|
||||
if (tm & TRACK_MODIFIER_INV)
|
||||
modename=F("DCX");
|
||||
else
|
||||
modename=F("DC");
|
||||
@@ -631,23 +652,25 @@ void TrackManager::setJoinRelayPin(byte joinRelayPin) {
|
||||
|
||||
void TrackManager::setJoin(bool joined) {
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
if (joined) {
|
||||
if (joined) { // if we go into joined mode (PROG acts as MAIN)
|
||||
FOR_EACH_TRACK(t) {
|
||||
if (track[t]->getMode() & TRACK_MODE_PROG) {
|
||||
tempProgTrack = t;
|
||||
if (track[t]->getMode() & TRACK_MODE_PROG) { // find PROG track
|
||||
tempProgTrack = t; // remember PROG track
|
||||
setTrackMode(t, TRACK_MODE_MAIN);
|
||||
break;
|
||||
track[t]->setPower(POWERMODE::ON); // if joined, always on
|
||||
break; // there is only one prog track, done
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (tempProgTrack != MAX_TRACKS+1) {
|
||||
// as setTrackMode with TRACK_MODE_PROG defaults to
|
||||
// power off, we will take the current power state
|
||||
// of our track and then preserve that state.
|
||||
POWERMODE tPTmode = track[tempProgTrack]->getPower(); //get current power status of this track
|
||||
setTrackMode(tempProgTrack, TRACK_MODE_PROG);
|
||||
track[tempProgTrack]->setPower(tPTmode); //set track status as it was before
|
||||
// setTrackMode defaults to power off, so we
|
||||
// need to preserve that state.
|
||||
POWERMODE tPTmode = track[tempProgTrack]->getPower(); // get current power status of this track
|
||||
setTrackMode(tempProgTrack, TRACK_MODE_PROG); // set track mode back to prog
|
||||
track[tempProgTrack]->setPower(tPTmode); // set power status as it was before
|
||||
tempProgTrack = MAX_TRACKS+1;
|
||||
} else {
|
||||
DIAG(F("Unjoin but no remembered prog track"));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@@ -1,7 +1,7 @@
|
||||
/*
|
||||
* © 2021 Neil McKechnie
|
||||
* © 2021 Mike S
|
||||
* © 2020-2022 Harald Barth
|
||||
* © 2020-2024 Harald Barth
|
||||
* © 2020-2021 M Steve Todd
|
||||
* © 2020-2021 Chris Harlow
|
||||
* All rights reserved.
|
||||
@@ -97,12 +97,18 @@ WiThrottle::WiThrottle( int wificlientid) {
|
||||
nextThrottle=firstThrottle;
|
||||
firstThrottle= this;
|
||||
clientid=wificlientid;
|
||||
#ifdef HEARTBEAT_CRITICAL
|
||||
heartBeatEnable=true; // do not run without heartbeat
|
||||
#else
|
||||
heartBeatEnable=false; // until client turns it on
|
||||
#endif
|
||||
mostRecentCab=0;
|
||||
for (int loco=0;loco<MAX_MY_LOCO; loco++) myLocos[loco].throttle='\0';
|
||||
}
|
||||
|
||||
WiThrottle::~WiThrottle() {
|
||||
// emergency stop any loco that was controlled from this throttle
|
||||
eStop();
|
||||
if (Diag::WITHROTTLE) DIAG(F("Deleting WiThrottle client %d"),this->clientid);
|
||||
if (firstThrottle== this) {
|
||||
firstThrottle=this->nextThrottle;
|
||||
@@ -140,7 +146,12 @@ void WiThrottle::parse(RingStream * stream, byte * cmdx) {
|
||||
switch (cmd[0]) {
|
||||
case '*': // heartbeat control
|
||||
if (cmd[1]=='+') heartBeatEnable=true;
|
||||
else if (cmd[1]=='-') heartBeatEnable=false;
|
||||
else if (cmd[1]=='-') {
|
||||
#ifdef HEARTBEAT_CRITICAL
|
||||
eStop();
|
||||
#endif
|
||||
heartBeatEnable=false;
|
||||
}
|
||||
break;
|
||||
case 'P':
|
||||
if (cmd[1]=='P' && cmd[2]=='A' ) { //PPA power mode
|
||||
@@ -303,10 +314,14 @@ void WiThrottle::locoAction(RingStream * stream, byte* aval, char throttleChar,
|
||||
switch (aval[0]) {
|
||||
case 'V': // Vspeed
|
||||
{
|
||||
int witSpeed=getInt(aval+1);
|
||||
uint8_t dccSpeed = WiTToDCCSpeed(getInt(aval+1));
|
||||
#ifdef HEARTBEAT_CRITICAL
|
||||
if (heartBeatEnable == false) // if there is no heartBeat, keep throttle at
|
||||
dccSpeed = 1; // emegency stop (dccSpeed 1)
|
||||
#endif
|
||||
LOOPLOCOS(throttleChar, cab) {
|
||||
mostRecentCab=myLocos[loco].cab;
|
||||
DCC::setThrottle(myLocos[loco].cab, WiTToDCCSpeed(witSpeed), DCC::getThrottleDirection(myLocos[loco].cab));
|
||||
DCC::setThrottle(myLocos[loco].cab, dccSpeed, DCC::getThrottleDirection(myLocos[loco].cab));
|
||||
// SetThrottle will cause speed change broadcast
|
||||
}
|
||||
}
|
||||
@@ -322,6 +337,15 @@ void WiThrottle::locoAction(RingStream * stream, byte* aval, char throttleChar,
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'f': // Function key set, force function variant
|
||||
{
|
||||
bool pressed=aval[1]=='1';
|
||||
int fKey = getInt(aval+2);
|
||||
LOOPLOCOS(throttleChar, cab) {
|
||||
DCC::setFn(myLocos[loco].cab,fKey, pressed);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'q':
|
||||
if (aval[1]=='V' || aval[1]=='R' ) { //qV or qR
|
||||
// just flag the loco for broadcast and it will happen.
|
||||
@@ -385,19 +409,28 @@ void WiThrottle::loop(RingStream * stream) {
|
||||
wt->checkHeartbeat(stream);
|
||||
}
|
||||
|
||||
void WiThrottle::eStop() {
|
||||
LOOPLOCOS('*', -1) {
|
||||
if (myLocos[loco].throttle!='\0') {
|
||||
if (Diag::WITHROTTLE) DIAG(F("%l eStopping cab %d"),millis(),myLocos[loco].cab);
|
||||
DCC::setThrottle(myLocos[loco].cab, 1, DCC::getThrottleDirection(myLocos[loco].cab)); // speed 1 is eStop
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void WiThrottle::checkHeartbeat(RingStream * stream) {
|
||||
// if eStop time passed... eStop any locos still assigned to this client and then drop the connection
|
||||
if(heartBeatEnable && (millis()-heartBeat > ESTOP_SECONDS*1000)) {
|
||||
if (Diag::WITHROTTLE) DIAG(F("%l WiThrottle(%d) eStop(%ds) timeout, drop connection"), millis(), clientid, ESTOP_SECONDS);
|
||||
LOOPLOCOS('*', -1) {
|
||||
if (myLocos[loco].throttle!='\0') {
|
||||
if (Diag::WITHROTTLE) DIAG(F("%l eStopping cab %d"),millis(),myLocos[loco].cab);
|
||||
DCC::setThrottle(myLocos[loco].cab, 1, DCC::getThrottleDirection(myLocos[loco].cab)); // speed 1 is eStop
|
||||
heartBeat=millis(); // We have just stopped everyting, we don't need to do that again at next loop.
|
||||
}
|
||||
if (Diag::WITHROTTLE) DIAG(F("%l WiThrottle(%d) eStop(%ds) timeout"), millis(), clientid, ESTOP_SECONDS);
|
||||
if (missedHeartbeatCounter++ > 10) {
|
||||
if (Diag::WITHROTTLE) DIAG(F("Too many missed heartbeats, drop connection"));
|
||||
delete this;
|
||||
return;
|
||||
}
|
||||
// if it does come back, the throttle should re-acquire
|
||||
delete this;
|
||||
eStop();
|
||||
heartBeat=millis(); // We have just stopped everyting, we don't need to do that again at next loop.
|
||||
// Destructor will take care of estop. If it does come back, the throttle should re-acquire
|
||||
// delete this;
|
||||
return;
|
||||
}
|
||||
|
||||
|
13
WiThrottle.h
13
WiThrottle.h
@@ -1,6 +1,7 @@
|
||||
/*
|
||||
* © 2021 Mike S
|
||||
* © 2020-2021 Chris Harlow
|
||||
* © 2024 Harald Barth
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of Asbelos DCC API
|
||||
@@ -20,7 +21,7 @@
|
||||
*/
|
||||
#ifndef WiThrottle_h
|
||||
#define WiThrottle_h
|
||||
|
||||
#include "defines.h"
|
||||
#include "RingStream.h"
|
||||
|
||||
struct MYLOCO {
|
||||
@@ -45,9 +46,15 @@ class WiThrottle {
|
||||
~WiThrottle();
|
||||
|
||||
static const int MAX_MY_LOCO=10; // maximum number of locos assigned to a single client
|
||||
#ifdef HEARTBEAT_CRITICAL
|
||||
static const int HEARTBEAT_SECONDS=1;
|
||||
static const int HEARTBEAT_PRELOAD=2;
|
||||
static const int ESTOP_SECONDS=3;
|
||||
#else
|
||||
static const int HEARTBEAT_SECONDS=10; // heartbeat at 10 secs to provide messaging transport
|
||||
static const int HEARTBEAT_PRELOAD=2; // request fast callback when connecting multiple messages
|
||||
static const int ESTOP_SECONDS=20; // eStop if no incoming messages for more than 8secs
|
||||
static const int ESTOP_SECONDS=20; // eStop if no incoming messages for more than 20secs
|
||||
#endif
|
||||
static WiThrottle* firstThrottle;
|
||||
static int getInt(byte * cmd);
|
||||
static int getLocoId(byte * cmd);
|
||||
@@ -62,6 +69,7 @@ class WiThrottle {
|
||||
MYLOCO myLocos[MAX_MY_LOCO];
|
||||
bool heartBeatEnable;
|
||||
unsigned long heartBeat;
|
||||
uint8_t missedHeartbeatCounter = 0;
|
||||
bool introSent=false;
|
||||
bool turnoutsSent=false;
|
||||
bool rosterSent=false;
|
||||
@@ -75,6 +83,7 @@ class WiThrottle {
|
||||
void multithrottle(RingStream * stream, byte * cmd);
|
||||
void locoAction(RingStream * stream, byte* aval, char throttleChar, int cab);
|
||||
void accessory(RingStream *, byte* cmd);
|
||||
void eStop();
|
||||
void checkHeartbeat(RingStream * stream);
|
||||
void markForBroadcast2(int cab);
|
||||
void sendIntro(Print * stream);
|
||||
|
@@ -23,13 +23,13 @@
|
||||
#include <vector>
|
||||
#include "defines.h"
|
||||
#include "ESPmDNS.h"
|
||||
#include <WiFi.h>
|
||||
#include "esp_wifi.h"
|
||||
#include "WifiESP32.h"
|
||||
#include "DIAG.h"
|
||||
#include "RingStream.h"
|
||||
#include "CommandDistributor.h"
|
||||
#include "WiThrottle.h"
|
||||
#include "DCC.h"
|
||||
/*
|
||||
#include "soc/rtc_wdt.h"
|
||||
#include "esp_task_wdt.h"
|
||||
@@ -109,10 +109,13 @@ private:
|
||||
bool inUse;
|
||||
};
|
||||
|
||||
// file scope variables
|
||||
static std::vector<NetworkClient> clients; // a list to hold all clients
|
||||
static WiFiServer *server = NULL;
|
||||
static RingStream *outboundRing = new RingStream(10240);
|
||||
static bool APmode = false;
|
||||
// init of static class scope variables
|
||||
bool WifiESP::wifiUp = false;
|
||||
WiFiServer *WifiESP::server = NULL;
|
||||
|
||||
#ifdef WIFI_TASK_ON_CORE0
|
||||
void wifiLoop(void *){
|
||||
@@ -128,6 +131,30 @@ char asciitolower(char in) {
|
||||
return in;
|
||||
}
|
||||
|
||||
void WifiESP::teardown() {
|
||||
// stop all locos
|
||||
DCC::setThrottle(0,1,1); // this broadcasts speed 1(estop) and sets all reminders to speed 1.
|
||||
// terminate all clients connections
|
||||
while (!clients.empty()) {
|
||||
// pop_back() should invoke destructor which does stop()
|
||||
// on the underlying TCP connction
|
||||
clients.pop_back();
|
||||
}
|
||||
// stop server
|
||||
if (server != NULL) {
|
||||
server->stop();
|
||||
server->close();
|
||||
server->end();
|
||||
DIAG(F("server stop, close, end"));
|
||||
}
|
||||
// terminate MDNS anouncement
|
||||
mdns_service_remove_all();
|
||||
mdns_free();
|
||||
// stop WiFi
|
||||
WiFi.disconnect(true);
|
||||
wifiUp = false;
|
||||
}
|
||||
|
||||
bool WifiESP::setup(const char *SSid,
|
||||
const char *password,
|
||||
const char *hostname,
|
||||
@@ -136,8 +163,10 @@ bool WifiESP::setup(const char *SSid,
|
||||
const bool forceAP) {
|
||||
bool havePassword = true;
|
||||
bool haveSSID = true;
|
||||
bool wifiUp = false;
|
||||
// bool wifiUp = false;
|
||||
uint8_t tries = 40;
|
||||
if (wifiUp)
|
||||
teardown();
|
||||
|
||||
//#ifdef SERIAL_BT_COMMANDS
|
||||
//return false;
|
||||
@@ -147,6 +176,12 @@ bool WifiESP::setup(const char *SSid,
|
||||
// enableCoreWDT(1);
|
||||
// disableCoreWDT(0);
|
||||
|
||||
#ifdef WIFI_LED
|
||||
// Turn off Wifi LED
|
||||
pinMode(WIFI_LED, OUTPUT);
|
||||
digitalWrite(WIFI_LED, 0);
|
||||
#endif
|
||||
|
||||
// clean start
|
||||
WiFi.mode(WIFI_STA);
|
||||
WiFi.disconnect(true);
|
||||
@@ -247,12 +282,19 @@ bool WifiESP::setup(const char *SSid,
|
||||
// no idea to go on
|
||||
return false;
|
||||
}
|
||||
#ifdef WIFI_LED
|
||||
else{
|
||||
// Turn on Wifi connected LED
|
||||
digitalWrite(WIFI_LED, 1);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// Now Wifi is up, register the mDNS service
|
||||
if(!MDNS.begin(hostname)) {
|
||||
DIAG(F("Wifi setup failed to start mDNS"));
|
||||
}
|
||||
if(!MDNS.addService("withrottle", "tcp", 2560)) {
|
||||
if(!MDNS.addService("withrottle", "tcp", port)) {
|
||||
DIAG(F("Wifi setup failed to add withrottle service to mDNS"));
|
||||
}
|
||||
|
||||
|
@@ -22,6 +22,7 @@
|
||||
#ifndef WifiESP32_h
|
||||
#define WifiESP32_h
|
||||
|
||||
#include <WiFi.h>
|
||||
#include "FSH.h"
|
||||
|
||||
class WifiESP
|
||||
@@ -36,6 +37,9 @@ public:
|
||||
const bool forceAP);
|
||||
static void loop();
|
||||
private:
|
||||
static void teardown();
|
||||
static bool wifiUp;
|
||||
static WiFiServer *server;
|
||||
};
|
||||
#endif //WifiESP8266_h
|
||||
#endif //ESP8266
|
||||
|
@@ -1,4 +1,5 @@
|
||||
/*
|
||||
* © 2022-2024 Paul M. Antoine
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2022 Harald Barth
|
||||
* © 2020-2022 Chris Harlow
|
||||
@@ -70,9 +71,10 @@ Stream * WifiInterface::wifiStream;
|
||||
#define SERIAL3 Serial5
|
||||
#elif defined(ARDUINO_NUCLEO_F413ZH) || defined(ARDUINO_NUCLEO_F429ZI) \
|
||||
|| defined(ARDUINO_NUCLEO_F446ZE) || defined(ARDUINO_NUCLEO_F412ZG) \
|
||||
|| defined(ARDUINO_NUCLEO_F439ZI)
|
||||
#define NUM_SERIAL 2
|
||||
|| defined(ARDUINO_NUCLEO_F439ZI) || defined(ARDUINO_NUCLEO_F4X9ZI)
|
||||
#define NUM_SERIAL 3
|
||||
#define SERIAL1 Serial6
|
||||
#define SERIAL3 Serial2
|
||||
#else
|
||||
#warning This variant of Nucleo not yet explicitly supported
|
||||
#endif
|
||||
|
@@ -307,11 +307,21 @@ The configuration file for DCC-EX Command Station
|
||||
//
|
||||
//#define SERIAL_BT_COMMANDS
|
||||
|
||||
// BOOSTER PIN INPUT ON ESP32
|
||||
// BOOSTER PIN INPUT ON ESP32 CS
|
||||
// On ESP32 you have the possibility to define a pin as booster input
|
||||
// Arduio pin D2 is GPIO 26 on ESPDuino32
|
||||
//
|
||||
// Arduino pin D2 is GPIO 26 is Booster Input on ESPDuino32
|
||||
//#define BOOSTER_INPUT 26
|
||||
//
|
||||
// GPIO 32 is Booster Input on EX-CSB1
|
||||
//#define BOOSTER_INPUT 32
|
||||
|
||||
// ESP32 LED Wifi Indicator
|
||||
// GPIO 2 on ESPduino32
|
||||
//#define WIFI_LED 2
|
||||
//
|
||||
// GPIO 33 on EX-CSB1
|
||||
//#define WIFI_LED 33
|
||||
|
||||
// SABERTOOTH
|
||||
//
|
||||
|
112
platformio.ini
112
platformio.ini
@@ -104,10 +104,35 @@ lib_deps =
|
||||
${env.lib_deps}
|
||||
arduino-libraries/Ethernet
|
||||
SPI
|
||||
MDNS_Generic
|
||||
|
||||
lib_ignore = WiFi101
|
||||
WiFi101_Generic
|
||||
WiFiEspAT
|
||||
WiFiMulti_Generic
|
||||
WiFiNINA_Generic
|
||||
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
build_flags =
|
||||
|
||||
[env:mega2560-eth]
|
||||
platform = atmelavr
|
||||
board = megaatmega2560
|
||||
framework = arduino
|
||||
lib_deps =
|
||||
${env.lib_deps}
|
||||
arduino-libraries/Ethernet
|
||||
MDNS_Generic
|
||||
SPI
|
||||
lib_ignore = WiFi101
|
||||
WiFi101_Generic
|
||||
WiFiEspAT
|
||||
WiFiMulti_Generic
|
||||
WiFiNINA_Generic
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
|
||||
[env:mega328]
|
||||
platform = atmelavr
|
||||
board = uno
|
||||
@@ -164,7 +189,11 @@ monitor_echo = yes
|
||||
build_flags = -mcall-prologues
|
||||
|
||||
[env:ESP32]
|
||||
platform = espressif32
|
||||
; Lock version to 6.7.0 as that is
|
||||
; Arduino v2.0.16 (based on IDF v4.4.7)
|
||||
; which is the latest version based
|
||||
; on IDF v4. We can not use IDF v5.
|
||||
platform = espressif32 @ 6.7.0
|
||||
board = esp32dev
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
@@ -173,7 +202,7 @@ monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
|
||||
[env:Nucleo-F411RE]
|
||||
platform = ststm32
|
||||
platform = ststm32 @ 17.6.0
|
||||
board = nucleo_f411re
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
@@ -182,7 +211,7 @@ monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
|
||||
[env:Nucleo-F446RE]
|
||||
platform = ststm32
|
||||
platform = ststm32 @ 17.6.0
|
||||
board = nucleo_f446re
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
@@ -194,7 +223,7 @@ monitor_echo = yes
|
||||
; tested as yet
|
||||
;
|
||||
[env:Nucleo-F401RE]
|
||||
platform = ststm32
|
||||
platform = ststm32 @ 17.6.0
|
||||
board = nucleo_f401re
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
@@ -207,7 +236,7 @@ monitor_echo = yes
|
||||
; installed before you can let PlatformIO see this
|
||||
;
|
||||
; [env:Nucleo-F413ZH]
|
||||
; platform = ststm32
|
||||
; platform = ststm32 @ 17.6.0
|
||||
; board = nucleo_f413zh
|
||||
; framework = arduino
|
||||
; lib_deps = ${env.lib_deps}
|
||||
@@ -218,21 +247,21 @@ monitor_echo = yes
|
||||
; Commented out by default as the F446ZE needs variant files
|
||||
; installed before you can let PlatformIO see this
|
||||
;
|
||||
; [env:Nucleo-F446ZE]
|
||||
; platform = ststm32
|
||||
; board = nucleo_f446ze
|
||||
; framework = arduino
|
||||
; lib_deps = ${env.lib_deps}
|
||||
; build_flags = -std=c++17 -Os -g2 -Wunused-variable
|
||||
; monitor_speed = 115200
|
||||
; monitor_echo = yes
|
||||
[env:Nucleo-F446ZE]
|
||||
platform = ststm32 @ 17.6.0
|
||||
board = nucleo_f446ze
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
build_flags = -std=c++17 -Os -g2 -Wunused-variable
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
|
||||
; Commented out by default as the F412ZG needs variant files
|
||||
; installed before you can let PlatformIO see this
|
||||
;
|
||||
; [env:Nucleo-F412ZG]
|
||||
; platform = ststm32
|
||||
; board = blah_f412zg
|
||||
; platform = ststm32 @ 17.6.0
|
||||
; board = nucleo_f412zg
|
||||
; framework = arduino
|
||||
; lib_deps = ${env.lib_deps}
|
||||
; build_flags = -std=c++17 -Os -g2 -Wunused-variable
|
||||
@@ -242,18 +271,47 @@ monitor_echo = yes
|
||||
|
||||
; Experimental - Ethernet work still in progress
|
||||
;
|
||||
; [env:Nucleo-F429ZI]
|
||||
; platform = ststm32
|
||||
; board = nucleo_f429zi
|
||||
; framework = arduino
|
||||
; lib_deps = ${env.lib_deps}
|
||||
; arduino-libraries/Ethernet @ ^2.0.1
|
||||
; stm32duino/STM32Ethernet @ ^1.3.0
|
||||
; stm32duino/STM32duino LwIP @ ^2.1.2
|
||||
; build_flags = -std=c++17 -Os -g2 -Wunused-variable
|
||||
; monitor_speed = 115200
|
||||
; monitor_echo = yes
|
||||
; upload_protocol = stlink
|
||||
[env:Nucleo-F429ZI]
|
||||
platform = ststm32 @ 17.6.0
|
||||
board = nucleo_f429zi
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
stm32duino/STM32Ethernet @ ^1.4.0
|
||||
stm32duino/STM32duino LwIP @ ^2.1.3
|
||||
MDNS_Generic
|
||||
lib_ignore = WiFi101
|
||||
WiFi101_Generic
|
||||
WiFiEspAT
|
||||
WiFiMulti_Generic
|
||||
WiFiNINA_Generic
|
||||
build_flags = -std=c++17 -Os -g2 -Wunused-variable
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
upload_protocol = stlink
|
||||
|
||||
; Experimental - Ethernet work still in progress
|
||||
;
|
||||
[env:Nucleo-F439ZI]
|
||||
platform = ststm32 @ 17.6.0
|
||||
; board = nucleo_f439zi
|
||||
; Temporarily treat it as an F429ZI (they are code compatible) until
|
||||
; the PR to PlatformIO to update the F439ZI JSON file is available
|
||||
; PMA - 28-Sep-2024
|
||||
board = nucleo_f429zi
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
stm32duino/STM32Ethernet @ ^1.4.0
|
||||
stm32duino/STM32duino LwIP @ ^2.1.3
|
||||
MDNS_Generic
|
||||
lib_ignore = WiFi101
|
||||
WiFi101_Generic
|
||||
WiFiEspAT
|
||||
WiFiMulti_Generic
|
||||
WiFiNINA_Generic
|
||||
build_flags = -std=c++17 -Os -g2 -Wunused-variable
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
upload_protocol = stlink
|
||||
|
||||
[env:Teensy3_2]
|
||||
platform = teensy
|
||||
|
49
version.h
49
version.h
@@ -3,7 +3,54 @@
|
||||
|
||||
#include "StringFormatter.h"
|
||||
|
||||
#define VERSION "5.2.54"
|
||||
#define VERSION "5.2.86"
|
||||
// 5.2.86 - add HEARTBEAT_CRITICAL to enforce heartbeat check
|
||||
// 5.2.85 - IO_TM1638 driver, SEG7 Exrail macro and _s7 segment pattern generator.
|
||||
// 5.2.84 - Fix TrackManager setDCCSignal and setPROGSignal for STM32 shadowing of PORTG/PORTH - this time it really is correct!
|
||||
// 5.2.83 - Various STM32 related fixes for serial ports, I2C pullups now turned off, and shadowing of PORTG/PORTH for TrackManager now correct
|
||||
// 5.2.82 - TrackManager and EXRAIL: Introduce more consistent names for <= ...> and SET_TRACK
|
||||
// 5.2.81 - STM32 Ethernet boards support, also now have specific EX8874 motor driver definition
|
||||
// 5.2.80 - EthernetInterface upgrade, including STM32 Ethernet support
|
||||
// 5.2.79 - serial manager loop that handles quoted strings
|
||||
// - WiFiESP32 reconfig
|
||||
// 5.2.78 - NeoPixel support.
|
||||
// - <o command
|
||||
// - HAL driver
|
||||
// - EXRAIL NEOPIXEL and NEOPIXEL_SIGNAL
|
||||
// 5.2.77 - Withrottle: Implement "force function" subcommand "f"
|
||||
// 5.2.76 - Bugfix: EXRAIL: Catch CV read errors in the callback
|
||||
// 5.2.75 - Bugfix: Serial lines 4 to 6 OK
|
||||
// 5.2.74 - Bugfix: ESP32 turn on the joined prog (as main) again after a prog operation
|
||||
// 5.2.73 - Bugfix: STM32 further fixes to shadowPORT entries in TrackManager.cpp for PORTG and PORTH
|
||||
// 5.2.72 - Bugfix: added shadowPORT entries in TrackManager.cpp for PORTG and PORTH on STM32, fixed typo in MotorDriver.cpp
|
||||
// 5.2.71 - Broadcasts of loco forgets.
|
||||
// 5.2.70 - IO_RocoDriver renamed to IO_EncoderThrottle.
|
||||
// - and included in IODEvice.h (circular dependency removed)
|
||||
// 5.2.69 - IO_RocoDriver. Direct drive train with rotary encoder hw.
|
||||
// 5.2.68 - Revert function map to signed (from 5.2.66) to avoid
|
||||
// incompatibilities with ED etc for F31 frequency flag.
|
||||
// 5.2.67 - EXRAIL AFTER optional debounce time variable (default 500mS)
|
||||
// - AFTER(42) == AFTER(42,500) sets time sensor must
|
||||
// - be continuously off.
|
||||
// 5.2.66 - <F cab DCFREQ 0..3>
|
||||
// - EXRAIL SETFREQ drop loco param (breaking since 5.2.28)
|
||||
// 5.2.65 - Speedup Exrail SETFREQ
|
||||
// 5.2.64 - Bugfix: <0 PROG> updated to undo JOIN
|
||||
// 5.2.63 - Implement WIFI_LED for ESP32, ESPduino32 and EX-CSB1, that is turned on when STA mode connects or AP mode is up
|
||||
// - Add BOOSTER_INPUT definitions for ESPduino32 and EX-CSB1 to config.example.h
|
||||
// - Add WIFI_LED definitions for ESPduino32 and EX-CSB1 to config.example.h
|
||||
// 5.2.62 - Allow acks way longer than standard
|
||||
// 5.2.61 - Merg CBUS ACON/ACOF/ONACON/ONACOF Adapter interface.
|
||||
// - LCC Adapter interface throttled startup,
|
||||
// (Breaking change with Adapter base code)
|
||||
// 5.2.60 - Bugfix: Opcode AFTEROVERLOAD does not have an argument that is a pin and needs to be initialized
|
||||
// - Remove inrush throttle after half good time so that we go to mode overload if problem persists
|
||||
// 5.2.59 - STM32 bugfix correct Serial1 definition for Nucleo-F401RE
|
||||
// - STM32 add support for ARDUINO_NUCLEO_F4X9ZI type to span F429/F439 in upcoming STM32duino release v2.8 as a result of our PR
|
||||
// 5.2.58 - EXRAIL ALIAS allows named pins
|
||||
// 5.2.57 - Bugfix autoreverse: Apply mode by binart bit match and not by equality
|
||||
// 5.2.56 - Bugfix and refactor for EXRAIL getSignalSlot
|
||||
// 5.2.55 - Move EXRAIL isSignal() to public to allow use in STEALTH call
|
||||
// 5.2.54 - Bugfix for EXRAIL signal handling for active high
|
||||
// 5.2.53 - Bugfix for EX-Fastclock, call I2CManager.begin() before checking I2C address
|
||||
// 5.2.52 - Bugfix for ADCee() to handle ADC2 and ADC3 channel inputs on F446ZE and others
|
||||
|
Reference in New Issue
Block a user