mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2025-07-31 03:13:45 +02:00
Compare commits
2 Commits
Author | SHA1 | Date | |
---|---|---|---|
|
8d9067c86b | ||
|
e7ed9ffbde |
@@ -32,10 +32,6 @@
|
||||
#include "TrackManager.h"
|
||||
#include "StringFormatter.h"
|
||||
|
||||
#ifdef Z21_PROTOCOL
|
||||
#include "Z21Throttle.h"
|
||||
#endif
|
||||
|
||||
// variables to hold clock time
|
||||
int16_t lastclocktime;
|
||||
int8_t lastclockrate;
|
||||
@@ -153,9 +149,6 @@ void CommandDistributor::broadcastToClients(clientType type) {
|
||||
|
||||
// Public broadcast functions below
|
||||
void CommandDistributor::broadcastSensor(int16_t id, bool on ) {
|
||||
#ifdef Z21_PROTOCOL
|
||||
Z21Throttle::broadcastNotifySensor(id, on);
|
||||
#endif
|
||||
broadcastReply(COMMAND_TYPE, F("<%c %d>\n"), on?'Q':'q', id);
|
||||
}
|
||||
|
||||
@@ -163,9 +156,6 @@ void CommandDistributor::broadcastTurnout(int16_t id, bool isClosed ) {
|
||||
// For DCC++ classic compatibility, state reported to JMRI is 1 for thrown and 0 for closed;
|
||||
// The string below contains serial and Withrottle protocols which should
|
||||
// be safe for both types.
|
||||
#ifdef Z21_PROTOCOL
|
||||
Z21Throttle::broadcastNotifyTurnout(id, isClosed);
|
||||
#endif
|
||||
broadcastReply(COMMAND_TYPE, F("<H %d %d>\n"),id, !isClosed);
|
||||
#ifdef CD_HANDLE_RING
|
||||
broadcastReply(WITHROTTLE_TYPE, F("PTA%c%d\n"), isClosed?'2':'4', id);
|
||||
@@ -320,11 +310,6 @@ void CommandDistributor::broadcastRaw(clientType type, char * msg) {
|
||||
broadcastReply(type, F("%s"),msg);
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastMessage(char * message) {
|
||||
broadcastReply(COMMAND_TYPE, F("<m \"%s\">\n"),message);
|
||||
broadcastReply(WITHROTTLE_TYPE, F("Hm%s\n"),message);
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastTrackState(const FSH* format, byte trackLetter, const FSH *modename, int16_t dcAddr) {
|
||||
broadcastReply(COMMAND_TYPE, format, trackLetter, modename, dcAddr);
|
||||
}
|
||||
|
@@ -60,7 +60,6 @@ public :
|
||||
static void forget(byte clientId);
|
||||
static void broadcastRouteState(uint16_t routeId,byte state);
|
||||
static void broadcastRouteCaption(uint16_t routeId,const FSH * caption);
|
||||
static void broadcastMessage(char * message);
|
||||
|
||||
// Handling code for virtual LCD receiver.
|
||||
static Print * getVirtualLCDSerial(byte screen, byte row);
|
||||
|
@@ -65,9 +65,6 @@
|
||||
#ifdef EXRAIL_WARNING
|
||||
#warning You have myAutomation.h but your hardware has not enough memory to do that, so EX-RAIL DISABLED
|
||||
#endif
|
||||
// compile time check, passwords 1 to 7 chars do not work, so do not try to compile with them at all
|
||||
// remember trailing '\0', sizeof("") == 1.
|
||||
#define PASSWDCHECK(S) static_assert(sizeof(S) == 1 || sizeof(S) > 8, "Password shorter than 8 chars")
|
||||
|
||||
void setup()
|
||||
{
|
||||
@@ -105,12 +102,10 @@ void setup()
|
||||
// Start Ethernet if it exists
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
#if WIFI_ON
|
||||
PASSWDCHECK(WIFI_PASSWORD); // compile time check
|
||||
WifiInterface::setup(WIFI_SERIAL_LINK_SPEED, F(WIFI_SSID), F(WIFI_PASSWORD), F(WIFI_HOSTNAME), IP_PORT, WIFI_CHANNEL, WIFI_FORCE_AP);
|
||||
#endif // WIFI_ON
|
||||
#else
|
||||
// ESP32 needs wifi on always
|
||||
PASSWDCHECK(WIFI_PASSWORD); // compile time check
|
||||
WifiESP::setup(WIFI_SSID, WIFI_PASSWORD, WIFI_HOSTNAME, IP_PORT, WIFI_CHANNEL, WIFI_FORCE_AP);
|
||||
#endif // ARDUINO_ARCH_ESP32
|
||||
|
||||
|
101
DCC.cpp
101
DCC.cpp
@@ -153,22 +153,6 @@ uint8_t DCC::getThrottleSpeedByte(int cab) {
|
||||
return speedTable[reg].speedCode;
|
||||
}
|
||||
|
||||
// returns 0 to 7 for frequency
|
||||
uint8_t DCC::getThrottleFrequency(int cab) {
|
||||
#if defined(ARDUINO_AVR_UNO)
|
||||
(void)cab;
|
||||
return 0;
|
||||
#else
|
||||
int reg=lookupSpeedTable(cab);
|
||||
if (reg<0)
|
||||
return 0; // use default frequency
|
||||
// shift out first 29 bits so we have the 3 "frequency bits" left
|
||||
uint8_t res = (uint8_t)(speedTable[reg].functions >>29);
|
||||
//DIAG(F("Speed table %d functions %l shifted %d"), reg, speedTable[reg].functions, res);
|
||||
return res;
|
||||
#endif
|
||||
}
|
||||
|
||||
// returns direction on loco
|
||||
// or true/forward on "loco not found"
|
||||
bool DCC::getThrottleDirection(int cab) {
|
||||
@@ -199,55 +183,43 @@ bool DCC::setFn( int cab, int16_t functionNumber, bool on) {
|
||||
b[nB++] = functionNumber >>7 ; // high order bits
|
||||
}
|
||||
DCCWaveform::mainTrack.schedulePacket(b, nB, 4);
|
||||
}
|
||||
// We use the reminder table up to 28 for normal functions.
|
||||
// We use 29 to 31 for DC frequency as well so up to 28
|
||||
// are "real" functions and 29 to 31 are frequency bits
|
||||
// controlled by function buttons
|
||||
if (functionNumber > 31)
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
int reg = lookupSpeedTable(cab);
|
||||
if (reg<0) return false;
|
||||
|
||||
// Take care of functions:
|
||||
// Set state of function
|
||||
uint32_t previous=speedTable[reg].functions;
|
||||
uint32_t funcmask = (1UL<<functionNumber);
|
||||
unsigned long previous=speedTable[reg].functions;
|
||||
unsigned long funcmask = (1UL<<functionNumber);
|
||||
if (on) {
|
||||
speedTable[reg].functions |= funcmask;
|
||||
} else {
|
||||
speedTable[reg].functions &= ~funcmask;
|
||||
}
|
||||
if (speedTable[reg].functions != previous) {
|
||||
if (functionNumber <= 28)
|
||||
updateGroupflags(speedTable[reg].groupFlags, functionNumber);
|
||||
updateGroupflags(speedTable[reg].groupFlags, functionNumber);
|
||||
CommandDistributor::broadcastLoco(reg);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Flip function state (used from withrottle protocol)
|
||||
// Flip function state
|
||||
void DCC::changeFn( int cab, int16_t functionNumber) {
|
||||
if (cab<=0 || functionNumber>31) return;
|
||||
if (cab<=0 || functionNumber>28) return;
|
||||
int reg = lookupSpeedTable(cab);
|
||||
if (reg<0) return;
|
||||
unsigned long funcmask = (1UL<<functionNumber);
|
||||
speedTable[reg].functions ^= funcmask;
|
||||
if (functionNumber <= 28) {
|
||||
updateGroupflags(speedTable[reg].groupFlags, functionNumber);
|
||||
}
|
||||
updateGroupflags(speedTable[reg].groupFlags, functionNumber);
|
||||
CommandDistributor::broadcastLoco(reg);
|
||||
}
|
||||
|
||||
// Report function state (used from withrottle protocol)
|
||||
// returns 0 false, 1 true or -1 for do not know
|
||||
int8_t DCC::getFn( int cab, int16_t functionNumber) {
|
||||
if (cab<=0 || functionNumber>31)
|
||||
return -1; // unknown
|
||||
int DCC::getFn( int cab, int16_t functionNumber) {
|
||||
if (cab<=0 || functionNumber>28) return -1; // unknown
|
||||
int reg = lookupSpeedTable(cab);
|
||||
if (reg<0)
|
||||
return -1;
|
||||
if (reg<0) return -1;
|
||||
|
||||
unsigned long funcmask = (1UL<<functionNumber);
|
||||
return (speedTable[reg].functions & funcmask)? 1 : 0;
|
||||
@@ -306,57 +278,6 @@ void DCC::setAccessory(int address, byte port, bool gate, byte onoff /*= 2*/) {
|
||||
}
|
||||
}
|
||||
|
||||
bool DCC::setExtendedAccessory(int16_t address, int16_t value, byte repeats) {
|
||||
|
||||
/* From https://www.nmra.org/sites/default/files/s-9.2.1_2012_07.pdf
|
||||
|
||||
The Extended Accessory Decoder Control Packet is included for the purpose of transmitting aspect control to signal
|
||||
decoders or data bytes to more complex accessory decoders. Each signal head can display one aspect at a time.
|
||||
{preamble} 0 10AAAAAA 0 0AAA0AA1 0 000XXXXX 0 EEEEEEEE 1
|
||||
|
||||
XXXXX is for a single head. A value of 00000 for XXXXX indicates the absolute stop aspect. All other aspects
|
||||
represented by the values for XXXXX are determined by the signaling system used and the prototype being
|
||||
modeled.
|
||||
|
||||
From https://normen.railcommunity.de/RCN-213.pdf:
|
||||
|
||||
More information is in RCN-213 about how the address bits are organized.
|
||||
preamble -0- 1 0 A7 A6 A5 A4 A3 A2 -0- 0 ^A10 ^A9 ^A8 0 A1 A0 1 -0- ....
|
||||
|
||||
Thus in byte packet form the format is 10AAAAAA, 0AAA0AA1, 000XXXXX
|
||||
|
||||
Die Adresse f<>r den ersten erweiterten Zubeh<65>rdecoder ist wie bei den einfachen
|
||||
Zubeh<EFBFBD>rdecodern die Adresse 4 = 1000-0001 0111-0001 . Diese Adresse wird in
|
||||
Anwenderdialogen als Adresse 1 dargestellt.
|
||||
|
||||
This means that the first address shown to the user as "1" is mapped
|
||||
to internal address 4.
|
||||
|
||||
Note that the Basic accessory format mentions "By convention these
|
||||
bits (bits 4-6 of the second data byte) are in ones complement" but
|
||||
this note is absent from the advanced packet description. The
|
||||
english translation does not mention that the address format for
|
||||
the advanced packet follows the one for the basic packet but
|
||||
according to the RCN-213 this is the case.
|
||||
|
||||
We allow for addresses from -3 to 2047-3 as that allows to address the
|
||||
whole range of the 11 bits sent to track.
|
||||
*/
|
||||
if ((address > 2044) || (address < -3)) return false; // 2047-3, 11 bits but offset 3
|
||||
if (value != (value & 0x1F)) return false; // 5 bits
|
||||
|
||||
address+=3; // +3 offset according to RCN-213
|
||||
byte b[3];
|
||||
b[0]= 0x80 // bits always on
|
||||
| ((address>>2) & 0x3F); // shift out 2, mask out used bits
|
||||
b[1]= 0x01 // bits always on
|
||||
| (((~(address>>8)) & 0x07)<<4) // shift out 8, invert, mask 3 bits, shift up 4
|
||||
| ((address & 0x03)<<1); // mask 2 bits, shift up 1
|
||||
b[2]=value;
|
||||
DCCWaveform::mainTrack.schedulePacket(b, sizeof(b), repeats);
|
||||
return true;
|
||||
}
|
||||
|
||||
//
|
||||
// writeCVByteMain: Write a byte with PoM on main. This writes
|
||||
// the 5 byte sized packet to implement this DCC function
|
||||
|
6
DCC.h
6
DCC.h
@@ -61,18 +61,16 @@ public:
|
||||
static void setThrottle(uint16_t cab, uint8_t tSpeed, bool tDirection);
|
||||
static int8_t getThrottleSpeed(int cab);
|
||||
static uint8_t getThrottleSpeedByte(int cab);
|
||||
static uint8_t getThrottleFrequency(int cab);
|
||||
static bool getThrottleDirection(int cab);
|
||||
static void writeCVByteMain(int cab, int cv, byte bValue);
|
||||
static void writeCVBitMain(int cab, int cv, byte bNum, bool bValue);
|
||||
static void setFunction(int cab, byte fByte, byte eByte);
|
||||
static bool setFn(int cab, int16_t functionNumber, bool on);
|
||||
static void changeFn(int cab, int16_t functionNumber);
|
||||
static int8_t getFn(int cab, int16_t functionNumber);
|
||||
static int getFn(int cab, int16_t functionNumber);
|
||||
static uint32_t getFunctionMap(int cab);
|
||||
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);
|
||||
static bool writeTextPacket(byte *b, int nBytes);
|
||||
|
||||
// ACKable progtrack calls bitresults callback 0,0 or -1, cv returns value or -1
|
||||
@@ -100,7 +98,7 @@ public:
|
||||
int loco;
|
||||
byte speedCode;
|
||||
byte groupFlags;
|
||||
uint32_t functions;
|
||||
unsigned long functions;
|
||||
};
|
||||
static LOCO speedTable[MAX_LOCOS];
|
||||
static int lookupSpeedTable(int locoId, bool autoCreate=true);
|
||||
|
@@ -45,7 +45,7 @@ Once a new OPCODE is decided upon, update this list.
|
||||
0, Track power off
|
||||
1, Track power on
|
||||
a, DCC accessory control
|
||||
A, DCC extended accessory control
|
||||
A,
|
||||
b, Write CV bit on main
|
||||
B, Write CV bit
|
||||
c, Request current command
|
||||
@@ -283,22 +283,25 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
return; // filterCallback asked us to ignore
|
||||
case 't': // THROTTLE <t [REGISTER] CAB SPEED DIRECTION>
|
||||
{
|
||||
if (params==1) { // <t cab> display state
|
||||
|
||||
int16_t slot=DCC::lookupSpeedTable(p[0],false);
|
||||
if (slot>=0) {
|
||||
DCC::LOCO * sp=&DCC::speedTable[slot];
|
||||
StringFormatter::send(stream,F("<l %d %d %d %l>\n"),
|
||||
sp->loco,slot,sp->speedCode,sp->functions);
|
||||
}
|
||||
else // send dummy state speed 0 fwd no functions.
|
||||
StringFormatter::send(stream,F("<l %d -1 128 0>\n"),p[0]);
|
||||
return;
|
||||
}
|
||||
|
||||
int16_t cab;
|
||||
int16_t tspeed;
|
||||
int16_t direction;
|
||||
|
||||
if (params==1) { // <t cab> display state
|
||||
int16_t slot=DCC::lookupSpeedTable(p[0],false);
|
||||
if (slot>=0)
|
||||
CommandDistributor::broadcastLoco(slot);
|
||||
else // send dummy state speed 0 fwd no functions.
|
||||
StringFormatter::send(stream,F("<l %d -1 128 0>\n"),p[0]);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
if (params == 4)
|
||||
{ // <t REGISTER CAB SPEED DIRECTION>
|
||||
// ignore register p[0]
|
||||
cab = p[1];
|
||||
tspeed = p[2];
|
||||
direction = p[3];
|
||||
@@ -381,13 +384,6 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
#endif
|
||||
}
|
||||
return;
|
||||
|
||||
case 'A': // EXTENDED ACCESSORY <A address value>
|
||||
// Note: if this happens to match a defined EXRAIL
|
||||
// DCCX_SIGNAL, then EXRAIL will have intercepted
|
||||
// this command alrerady.
|
||||
if (params==2 && DCC::setExtendedAccessory(p[0],p[1])) return;
|
||||
break;
|
||||
|
||||
case 'T': // TURNOUT <T ...>
|
||||
if (parseT(stream, params, p))
|
||||
@@ -579,13 +575,12 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
DCC::setThrottle(0,1,1); // this broadcasts speed 1(estop) and sets all reminders to speed 1.
|
||||
return;
|
||||
|
||||
#ifdef HAS_ENOUGH_MEMORY
|
||||
case 'c': // SEND METER RESPONSES <c>
|
||||
// No longer useful because of multiple tracks See <JG> and <JI>
|
||||
if (params>0) break;
|
||||
TrackManager::reportObsoleteCurrent(stream);
|
||||
return;
|
||||
#endif
|
||||
|
||||
case 'Q': // SENSORS <Q>
|
||||
Sensor::printAll(stream);
|
||||
return;
|
||||
@@ -1040,32 +1035,7 @@ bool DCCEXParser::parseC(Print *stream, int16_t params, int16_t p[]) {
|
||||
DCC::setGlobalSpeedsteps(128);
|
||||
DIAG(F("128 Speedsteps"));
|
||||
return true;
|
||||
#if defined(HAS_ENOUGH_MEMORY) && !defined(ARDUINO_ARCH_UNO)
|
||||
case "RAILCOM"_hk:
|
||||
{ // <C RAILCOM ON|OFF|DEBUG >
|
||||
if (params<2) return false;
|
||||
bool on=false;
|
||||
bool debug=false;
|
||||
switch (p[1]) {
|
||||
case "ON"_hk:
|
||||
case 1:
|
||||
on=true;
|
||||
break;
|
||||
case "DEBUG"_hk:
|
||||
on=true;
|
||||
debug=true;
|
||||
break;
|
||||
case "OFF"_hk:
|
||||
case 0:
|
||||
break;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
DIAG(F("Railcom %S")
|
||||
,DCCWaveform::setRailcom(on,debug)?F("ON"):F("OFF"));
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef DISABLE_PROG
|
||||
case "ACK"_hk: // <D ACK ON/OFF> <D ACK [LIMIT|MIN|MAX|RETRY] Value>
|
||||
if (params >= 3) {
|
||||
@@ -1117,21 +1087,6 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
|
||||
return true;
|
||||
|
||||
#ifdef HAS_ENOUGH_MEMORY
|
||||
#ifdef Z21_PROTOCOL
|
||||
case "Z21THROTTLE"_hk: // <D Z21THROTTLE ON/OFF>
|
||||
case "Z21THR"_hk: // <D Z21THR ON/OFF>
|
||||
Diag::Z21THROTTLE = onOff;
|
||||
return true;
|
||||
case "Z21THROTTLEVERBOSE"_hk: // <D Z21THROTTLEVERBOSE ON/OFF>
|
||||
case "Z21THRV"_hk: // <D Z21THV ON/OFF>
|
||||
Diag::Z21THROTTLEVERBOSE = onOff;
|
||||
return true;
|
||||
case "Z21THROTTLEDATA"_hk: // <D Z21THROTTLEDATA ON/OFF>
|
||||
case "Z21THRD"_hk: // <D Z21THD ON/OFF>
|
||||
Diag::Z21THROTTLEDATA = onOff;
|
||||
return true;
|
||||
#endif // Z21_PROTOCOL
|
||||
|
||||
case "WIFI"_hk: // <D WIFI ON/OFF>
|
||||
Diag::WIFI = onOff;
|
||||
return true;
|
||||
|
@@ -62,8 +62,6 @@ class DCCTimer {
|
||||
static bool isPWMPin(byte pin);
|
||||
static void setPWM(byte pin, bool high);
|
||||
static void clearPWM();
|
||||
static void startRailcomTimer(byte brakePin);
|
||||
static void ackRailcomTimer();
|
||||
static void DCCEXanalogWriteFrequency(uint8_t pin, uint32_t frequency);
|
||||
static void DCCEXanalogWrite(uint8_t pin, int value);
|
||||
|
||||
@@ -87,7 +85,6 @@ class DCCTimer {
|
||||
static void reset();
|
||||
|
||||
private:
|
||||
static void DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t frequency);
|
||||
static int freeMemory();
|
||||
static volatile int minimum_free_memory;
|
||||
static const int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle
|
||||
|
140
DCCTimerAVR.cpp
140
DCCTimerAVR.cpp
@@ -29,7 +29,6 @@
|
||||
#include <avr/boot.h>
|
||||
#include <avr/wdt.h>
|
||||
#include "DCCTimer.h"
|
||||
#include "DIAG.h"
|
||||
#ifdef DEBUG_ADC
|
||||
#include "TrackManager.h"
|
||||
#endif
|
||||
@@ -40,9 +39,6 @@ INTERRUPT_CALLBACK interruptHandler=0;
|
||||
#define TIMER1_A_PIN 11
|
||||
#define TIMER1_B_PIN 12
|
||||
#define TIMER1_C_PIN 13
|
||||
#define TIMER2_A_PIN 10
|
||||
#define TIMER2_B_PIN 9
|
||||
|
||||
#else
|
||||
#define TIMER1_A_PIN 9
|
||||
#define TIMER1_B_PIN 10
|
||||
@@ -59,67 +55,6 @@ void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||
interrupts();
|
||||
}
|
||||
|
||||
|
||||
void DCCTimer::startRailcomTimer(byte brakePin) {
|
||||
/* The Railcom timer is started in such a way that it
|
||||
- First triggers 28uS after the last TIMER1 tick.
|
||||
This provides an accurate offset (in High Accuracy mode)
|
||||
for the start of the Railcom cutout.
|
||||
- Sets the Railcom pin high at first tick,
|
||||
because its been setup with 100% PWM duty cycle.
|
||||
|
||||
- Cycles at 436uS so the second tick is the
|
||||
correct distance from the cutout.
|
||||
|
||||
- Waveform code is responsible for altering the PWM
|
||||
duty cycle to 0% any time between the first and last tick.
|
||||
(there will be 7 DCC timer1 ticks in which to do this.)
|
||||
|
||||
*/
|
||||
(void) brakePin; // Ignored... works on pin 9 only
|
||||
const int cutoutDuration = 430; // Desired interval in microseconds
|
||||
|
||||
// Set up Timer2 for CTC mode (Clear Timer on Compare Match)
|
||||
TCCR2A = 0; // Clear Timer2 control register A
|
||||
TCCR2B = 0; // Clear Timer2 control register B
|
||||
TCNT2 = 0; // Initialize Timer2 counter value to 0
|
||||
// Configure Phase and Frequency Correct PWM mode
|
||||
TCCR2A = (1 << COM2B1); // enable pwm on pin 9
|
||||
TCCR2A |= (1 << WGM20);
|
||||
|
||||
|
||||
// Set Timer 2 prescaler to 32
|
||||
TCCR2B = (1 << CS21) | (1 << CS20); // 32 prescaler
|
||||
|
||||
// Set the compare match value for desired interval
|
||||
OCR2A = (F_CPU / 1000000) * cutoutDuration / 64 - 1;
|
||||
|
||||
// Calculate the compare match value for desired duty cycle
|
||||
OCR2B = OCR2A+1; // set duty cycle to 100%= OCR2A)
|
||||
|
||||
// Enable Timer2 output on pin 9 (OC2B)
|
||||
DDRB |= (1 << DDB1);
|
||||
// TODO Fudge TCNT2 to sync with last tcnt1 tick + 28uS
|
||||
|
||||
// Previous TIMER1 Tick was at rising end-of-packet bit
|
||||
// Cutout starts half way through first preamble
|
||||
// that is 2.5 * 58uS later.
|
||||
// TCNT1 ticks 8 times / microsecond
|
||||
// auto microsendsToFirstRailcomTick=(58+58+29)-(TCNT1/8);
|
||||
// set the railcom timer counter allowing for phase-correct
|
||||
|
||||
// CHris's NOTE:
|
||||
// I dont kniow quite how this calculation works out but
|
||||
// it does seems to get a good answer.
|
||||
|
||||
TCNT2=193 + (ICR1 - TCNT1)/8;
|
||||
}
|
||||
|
||||
void DCCTimer::ackRailcomTimer() {
|
||||
OCR2B= 0x00; // brake pin pwm duty cycle 0 at next tick
|
||||
}
|
||||
|
||||
|
||||
// ISR called by timer interrupt every 58uS
|
||||
ISR(TIMER1_OVF_vect){ interruptHandler(); }
|
||||
|
||||
@@ -190,81 +125,6 @@ void DCCTimer::reset() {
|
||||
|
||||
}
|
||||
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, f);
|
||||
}
|
||||
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
|
||||
#if defined(ARDUINO_AVR_UNO)
|
||||
// Not worth doin something here as:
|
||||
// If we are on pin 9 or 10 we are on Timer1 and we can not touch Timer1 as that is our DCC source.
|
||||
// If we are on pin 5 or 6 we are on Timer 0 ad we can not touch Timer0 as that is millis() etc.
|
||||
// We are most likely not on pin 3 or 11 as no known motor shield has that as brake.
|
||||
#endif
|
||||
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
|
||||
// Speed mapping is done like this:
|
||||
// No functions buttons: 000 0 -> low 131Hz
|
||||
// Only F29 pressed 001 1 -> mid 490Hz
|
||||
// F30 with or w/o F29 01x 2-3 -> high 3400Hz
|
||||
// F31 with or w/o F29/30 1xx 4-7 -> supersonic 62500Hz
|
||||
uint8_t abits;
|
||||
uint8_t bbits;
|
||||
if (pin == 9 || pin == 10) { // timer 2 is different
|
||||
|
||||
if (fbits >= 4)
|
||||
abits = B00000011;
|
||||
else
|
||||
abits = B00000001;
|
||||
|
||||
if (fbits >= 4)
|
||||
bbits = B0001;
|
||||
else if (fbits >= 2)
|
||||
bbits = B0010;
|
||||
else if (fbits == 1)
|
||||
bbits = B0100;
|
||||
else // fbits == 0
|
||||
bbits = B0110;
|
||||
|
||||
TCCR2A = (TCCR2A & B11111100) | abits; // set WGM0 and WGM1
|
||||
TCCR2B = (TCCR2B & B11110000) | bbits; // set WGM2 and 3 bits of prescaler
|
||||
DIAG(F("Timer 2 A=%x B=%x"), TCCR2A, TCCR2B);
|
||||
|
||||
} else { // not timer 9 or 10
|
||||
abits = B01;
|
||||
|
||||
if (fbits >= 4)
|
||||
bbits = B1001;
|
||||
else if (fbits >= 2)
|
||||
bbits = B0010;
|
||||
else if (fbits == 1)
|
||||
bbits = B0011;
|
||||
else
|
||||
bbits = B0100;
|
||||
|
||||
switch (pin) {
|
||||
// case 9 and 10 taken care of above by if()
|
||||
case 6:
|
||||
case 7:
|
||||
case 8:
|
||||
// Timer4
|
||||
TCCR4A = (TCCR4A & B11111100) | abits; // set WGM0 and WGM1
|
||||
TCCR4B = (TCCR4B & B11100000) | bbits; // set WGM2 and WGM3 and divisor
|
||||
//DIAG(F("Timer 4 A=%x B=%x"), TCCR4A, TCCR4B);
|
||||
break;
|
||||
case 46:
|
||||
case 45:
|
||||
case 44:
|
||||
// Timer5
|
||||
TCCR5A = (TCCR5A & B11111100) | abits; // set WGM0 and WGM1
|
||||
TCCR5B = (TCCR5B & B11100000) | bbits; // set WGM2 and WGM3 and divisor
|
||||
//DIAG(F("Timer 5 A=%x B=%x"), TCCR5A, TCCR5B);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
|
||||
#define NUM_ADC_INPUTS 16
|
||||
#else
|
||||
|
@@ -83,18 +83,26 @@ int DCCTimer::freeMemory() {
|
||||
#include <soc/sens_struct.h>
|
||||
#undef ADC_INPUT_MAX_VALUE
|
||||
#define ADC_INPUT_MAX_VALUE 4095 // 12 bit ADC
|
||||
|
||||
#ifdef ARDUINO_ESP32S3_DEV
|
||||
#define pinToADC1Channel(X) (adc1_channel_t)((X)-1)
|
||||
#define MY_SAR_REG sar_meas1_ctrl2
|
||||
#else
|
||||
#define pinToADC1Channel(X) (adc1_channel_t)(((X) > 35) ? (X)-36 : (X)-28)
|
||||
#define MY_SAR_REG sar_meas_start1
|
||||
#endif
|
||||
|
||||
int IRAM_ATTR local_adc1_get_raw(int channel) {
|
||||
uint16_t adc_value;
|
||||
SENS.sar_meas_start1.sar1_en_pad = (1 << channel); // only one channel is selected
|
||||
SENS.MY_SAR_REG.sar1_en_pad = (1 << channel); // only one channel is selected
|
||||
while (SENS.sar_slave_addr1.meas_status != 0);
|
||||
SENS.sar_meas_start1.meas1_start_sar = 0;
|
||||
SENS.sar_meas_start1.meas1_start_sar = 1;
|
||||
while (SENS.sar_meas_start1.meas1_done_sar == 0);
|
||||
adc_value = SENS.sar_meas_start1.meas1_data_sar;
|
||||
SENS.MY_SAR_REG.meas1_start_sar = 0;
|
||||
SENS.MY_SAR_REG.meas1_start_sar = 1;
|
||||
while (SENS.MY_SAR_REG.meas1_done_sar == 0);
|
||||
adc_value = SENS.MY_SAR_REG.meas1_data_sar;
|
||||
return adc_value;
|
||||
}
|
||||
#undef MY_SAR_REG
|
||||
|
||||
#include "DCCTimer.h"
|
||||
INTERRUPT_CALLBACK interruptHandler=0;
|
||||
@@ -151,26 +159,10 @@ void DCCTimer::reset() {
|
||||
ESP.restart();
|
||||
}
|
||||
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
|
||||
if (f >= 16)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, f);
|
||||
else if (f == 7)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 62500);
|
||||
else if (f >= 4)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 32000);
|
||||
else if (f >= 3)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 16000);
|
||||
else if (f >= 2)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 3400);
|
||||
else if (f == 1)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 480);
|
||||
else
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 131);
|
||||
}
|
||||
|
||||
#include "esp32-hal.h"
|
||||
#include "soc/soc_caps.h"
|
||||
|
||||
|
||||
#ifdef SOC_LEDC_SUPPORT_HS_MODE
|
||||
#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM<<1)
|
||||
#else
|
||||
@@ -180,7 +172,7 @@ void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
|
||||
static int8_t pin_to_channel[SOC_GPIO_PIN_COUNT] = { 0 };
|
||||
static int cnt_channel = LEDC_CHANNELS;
|
||||
|
||||
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t frequency) {
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t frequency) {
|
||||
if (pin < SOC_GPIO_PIN_COUNT) {
|
||||
if (pin_to_channel[pin] != 0) {
|
||||
ledcSetup(pin_to_channel[pin], frequency, 8);
|
||||
|
@@ -80,15 +80,6 @@ extern char *__malloc_heap_start;
|
||||
interruptHandler();
|
||||
}
|
||||
|
||||
void DCCTimer::startRailcomTimer(byte brakePin) {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
(void) brakePin;
|
||||
}
|
||||
|
||||
void DCCTimer::ackRailcomTimer() {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
}
|
||||
|
||||
bool DCCTimer::isPWMPin(byte pin) {
|
||||
(void) pin;
|
||||
return false; // TODO what are the relevant pins?
|
||||
@@ -134,11 +125,6 @@ void DCCTimer::reset() {
|
||||
while(true){}
|
||||
}
|
||||
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
|
||||
}
|
||||
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
|
||||
}
|
||||
|
||||
int16_t ADCee::ADCmax() {
|
||||
return 4095;
|
||||
}
|
||||
|
@@ -76,15 +76,6 @@ void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||
interrupts();
|
||||
}
|
||||
|
||||
void DCCTimer::startRailcomTimer(byte brakePin) {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
(void) brakePin;
|
||||
}
|
||||
|
||||
void DCCTimer::ackRailcomTimer() {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
}
|
||||
|
||||
// Timer IRQ handlers replace the dummy handlers (in cortex_handlers)
|
||||
// copied from rf24 branch
|
||||
void TCC0_Handler() {
|
||||
@@ -165,11 +156,6 @@ void DCCTimer::reset() {
|
||||
while(true) {};
|
||||
}
|
||||
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
|
||||
}
|
||||
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
|
||||
}
|
||||
|
||||
#define NUM_ADC_INPUTS NUM_ANALOG_INPUTS
|
||||
|
||||
uint16_t ADCee::usedpins = 0;
|
||||
|
@@ -201,15 +201,6 @@ void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||
interrupts();
|
||||
}
|
||||
|
||||
void DCCTimer::startRailcomTimer(byte brakePin) {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
(void) brakePin;
|
||||
}
|
||||
|
||||
void DCCTimer::ackRailcomTimer() {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
}
|
||||
|
||||
bool DCCTimer::isPWMPin(byte pin) {
|
||||
//TODO: STM32 whilst this call to digitalPinHasPWM will reveal which pins can do PWM,
|
||||
// there's no support yet for High Accuracy, so for now return false
|
||||
@@ -266,23 +257,6 @@ void DCCTimer::reset() {
|
||||
while(true) {};
|
||||
}
|
||||
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
|
||||
if (f >= 16)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, f);
|
||||
else if (f == 7)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 62500);
|
||||
else if (f >= 4)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 32000);
|
||||
else if (f >= 3)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 16000);
|
||||
else if (f >= 2)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 3400);
|
||||
else if (f == 1)
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 480);
|
||||
else
|
||||
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 131);
|
||||
}
|
||||
|
||||
// TODO: rationalise the size of these... could really use sparse arrays etc.
|
||||
static HardwareTimer * pin_timer[100] = {0};
|
||||
static uint32_t channel_frequency[100] = {0};
|
||||
@@ -293,7 +267,7 @@ static uint32_t pin_channel[100] = {0};
|
||||
// sophisticated about detecting any clash between the timer we'd like to use for PWM and the ones
|
||||
// currently used for HA so they don't interfere with one another. For now we'll just make PWM
|
||||
// work well... then work backwards to integrate with HA mode if we can.
|
||||
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t frequency)
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t frequency)
|
||||
{
|
||||
if (pin_timer[pin] == NULL) {
|
||||
// Automatically retrieve TIM instance and channel associated to pin
|
||||
|
@@ -39,15 +39,6 @@ void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||
myDCCTimer.begin(interruptHandler, DCC_SIGNAL_TIME);
|
||||
}
|
||||
|
||||
void DCCTimer::startRailcomTimer(byte brakePin) {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
(void) brakePin;
|
||||
}
|
||||
|
||||
void DCCTimer::ackRailcomTimer() {
|
||||
// TODO: for intended operation see DCCTimerAVR.cpp
|
||||
}
|
||||
|
||||
bool DCCTimer::isPWMPin(byte pin) {
|
||||
//Teensy: digitalPinHasPWM, todo
|
||||
(void) pin;
|
||||
@@ -150,11 +141,6 @@ void DCCTimer::reset() {
|
||||
SCB_AIRCR = 0x05FA0004;
|
||||
}
|
||||
|
||||
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
|
||||
}
|
||||
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
|
||||
}
|
||||
|
||||
int16_t ADCee::ADCmax() {
|
||||
return 4095;
|
||||
}
|
||||
|
@@ -115,22 +115,8 @@ DCCWaveform::DCCWaveform( byte preambleBits, bool isMain) {
|
||||
bytes_sent = 0;
|
||||
bits_sent = 0;
|
||||
}
|
||||
|
||||
volatile bool DCCWaveform::railcomActive=false; // switched on by user
|
||||
volatile bool DCCWaveform::railcomDebug=false; // switched on by user
|
||||
|
||||
bool DCCWaveform::setRailcom(bool on, bool debug) {
|
||||
if (on) {
|
||||
// TODO check possible
|
||||
railcomActive=true;
|
||||
railcomDebug=debug;
|
||||
}
|
||||
else {
|
||||
railcomActive=false;
|
||||
railcomDebug=false;
|
||||
}
|
||||
return railcomActive;
|
||||
}
|
||||
|
||||
|
||||
#pragma GCC push_options
|
||||
#pragma GCC optimize ("-O3")
|
||||
@@ -138,16 +124,16 @@ void DCCWaveform::interrupt2() {
|
||||
// calculate the next bit to be sent:
|
||||
// set state WAVE_MID_1 for a 1=bit
|
||||
// or WAVE_HIGH_0 for a 0 bit.
|
||||
|
||||
if (remainingPreambles > 0 ) {
|
||||
state=WAVE_MID_1; // switch state to trigger LOW on next interrupt
|
||||
remainingPreambles--;
|
||||
|
||||
|
||||
// As we get to the end of the preambles, open the reminder window.
|
||||
// This delays any reminder insertion until the last moment so
|
||||
// that the reminder doesn't block a more urgent packet.
|
||||
reminderWindowOpen=transmitRepeats==0 && remainingPreambles<4 && remainingPreambles>1;
|
||||
if (remainingPreambles==1) promotePendingPacket();
|
||||
else if (remainingPreambles==10 && isMainTrack && railcomActive) DCCTimer::ackRailcomTimer();
|
||||
// Update free memory diagnostic as we don't have anything else to do this time.
|
||||
// Allow for checkAck and its called functions using 22 bytes more.
|
||||
else DCCTimer::updateMinimumFreeMemoryISR(22);
|
||||
@@ -171,12 +157,6 @@ void DCCWaveform::interrupt2() {
|
||||
bytes_sent = 0;
|
||||
// preamble for next packet will start...
|
||||
remainingPreambles = requiredPreambles;
|
||||
|
||||
// set the railcom coundown to trigger half way
|
||||
// through the first preamble bit.
|
||||
// Note.. we are still sending the last packet bit
|
||||
// and we then have to allow for the packet end bit
|
||||
if (isMainTrack && railcomActive) DCCTimer::startRailcomTimer(9);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -228,11 +208,7 @@ void DCCWaveform::promotePendingPacket() {
|
||||
|
||||
// nothing to do, just send idles or resets
|
||||
// Fortunately reset and idle packets are the same length
|
||||
// Note: If railcomDebug is on, then we send resets to the main
|
||||
// track instead of idles. This means that all data will be zeros
|
||||
// and only the porersets will be ones, making it much
|
||||
// easier to read on a logic analyser.
|
||||
memcpy( transmitPacket, (isMainTrack && (!railcomDebug)) ? idlePacket : resetPacket, sizeof(idlePacket));
|
||||
memcpy( transmitPacket, isMainTrack ? idlePacket : resetPacket, sizeof(idlePacket));
|
||||
transmitLength = sizeof(idlePacket);
|
||||
transmitRepeats = 0;
|
||||
if (getResets() < 250) sentResetsSincePacket++; // only place to increment (private!)
|
||||
@@ -294,7 +270,7 @@ void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repea
|
||||
// The resets will be zero not only now but as well repeats packets into the future
|
||||
clearResets(repeats+1);
|
||||
{
|
||||
int ret = 0;
|
||||
int ret;
|
||||
do {
|
||||
if(isMainTrack) {
|
||||
if (rmtMainChannel != NULL)
|
||||
@@ -321,10 +297,4 @@ bool DCCWaveform::isReminderWindowOpen() {
|
||||
void IRAM_ATTR DCCWaveform::loop() {
|
||||
DCCACK::checkAck(progTrack.getResets());
|
||||
}
|
||||
|
||||
bool DCCWaveform::setRailcom(bool on, bool debug) {
|
||||
// TODO... ESP32 railcom waveform
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
@@ -40,14 +40,7 @@ const byte MAX_PACKET_SIZE = 5; // NMRA standard extended packets, payload s
|
||||
|
||||
// The WAVE_STATE enum is deliberately numbered because a change of order would be catastrophic
|
||||
// to the transform array.
|
||||
enum WAVE_STATE : byte {
|
||||
WAVE_START=0, // wave going high at start of bit
|
||||
WAVE_MID_1=1, // middle of 1 bit
|
||||
WAVE_HIGH_0=2, // first part of 0 bit high
|
||||
WAVE_MID_0=3, // middle of 0 bit
|
||||
WAVE_LOW_0=4, // first part of 0 bit low
|
||||
WAVE_PENDING=5 // next bit not yet known
|
||||
};
|
||||
enum WAVE_STATE : byte {WAVE_START=0,WAVE_MID_1=1,WAVE_HIGH_0=2,WAVE_MID_0=3,WAVE_LOW_0=4,WAVE_PENDING=5};
|
||||
|
||||
// NOTE: static functions are used for the overall controller, then
|
||||
// one instance is created for each track.
|
||||
@@ -85,8 +78,6 @@ class DCCWaveform {
|
||||
void schedulePacket(const byte buffer[], byte byteCount, byte repeats);
|
||||
bool isReminderWindowOpen();
|
||||
void promotePendingPacket();
|
||||
static bool setRailcom(bool on, bool debug);
|
||||
static bool isRailcom() {return railcomActive;}
|
||||
|
||||
private:
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
@@ -112,9 +103,6 @@ class DCCWaveform {
|
||||
byte pendingPacket[MAX_PACKET_SIZE+1]; // +1 for checksum
|
||||
byte pendingLength;
|
||||
byte pendingRepeats;
|
||||
static volatile bool railcomActive; // switched on by user
|
||||
static volatile bool railcomDebug; // switched on by user
|
||||
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
static RMTChannel *rmtMainChannel;
|
||||
static RMTChannel *rmtProgChannel;
|
||||
|
95
EXRAIL2.cpp
95
EXRAIL2.cpp
@@ -669,45 +669,6 @@ void RMFT2::loop2() {
|
||||
}
|
||||
break;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case OPCODE_RESUME:
|
||||
pausingTask=NULL;
|
||||
driveLoco(speedo);
|
||||
@@ -839,14 +800,6 @@ void RMFT2::loop2() {
|
||||
DCC::setAccessory(addr,subaddr,active);
|
||||
break;
|
||||
}
|
||||
case OPCODE_ASPECT: {
|
||||
// operand is address<<5 | value
|
||||
int16_t address=operand>>5;
|
||||
byte aspect=operand & 0x1f;
|
||||
if (!signalAspectEvent(address,aspect))
|
||||
DCC::setExtendedAccessory(address,aspect);
|
||||
break;
|
||||
}
|
||||
|
||||
case OPCODE_FOLLOW:
|
||||
progCounter=routeLookup->find(operand);
|
||||
@@ -1108,7 +1061,7 @@ int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
if (diag) DIAG(F(" doSignal %d %x"),id,rag);
|
||||
|
||||
// Schedule any event handler for this signal change.
|
||||
// This will work even without a signal definition.
|
||||
// Thjis will work even without a signal definition.
|
||||
if (rag==SIGNAL_RED) onRedLookup->handleEvent(F("RED"),id);
|
||||
else if (rag==SIGNAL_GREEN) onGreenLookup->handleEvent(F("GREEN"),id);
|
||||
else onAmberLookup->handleEvent(F("AMBER"),id);
|
||||
@@ -1145,16 +1098,6 @@ int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (sigtype== DCCX_SIGNAL_FLAG) {
|
||||
// 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 & SIGNAL_ID_MASK,value);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
@@ -1188,38 +1131,6 @@ int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
return (flags[sigslot] & SIGNAL_MASK) == rag;
|
||||
}
|
||||
|
||||
|
||||
// signalAspectEvent returns true if the aspect is destined
|
||||
// for a defined DCCX_SIGNAL which will handle all the RAG flags
|
||||
// and ON* handlers.
|
||||
// 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);
|
||||
if (sigslot<0) return false; // this is not a defined signal
|
||||
int16_t sigpos=sigslot*8;
|
||||
VPIN sigid=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos);
|
||||
VPIN sigtype=sigid & ~SIGNAL_ID_MASK;
|
||||
if (sigtype!=DCCX_SIGNAL_FLAG) 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);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (aspect==GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+4)) {
|
||||
doSignal(sigid,SIGNAL_AMBER);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (aspect==GETHIGHFLASHW(RMFT2::SignalDefinitions,sigpos+6)) {
|
||||
doSignal(sigid,SIGNAL_GREEN);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false; // aspect is not a defined one
|
||||
}
|
||||
|
||||
void RMFT2::turnoutEvent(int16_t turnoutId, bool closed) {
|
||||
// Hunt for an ONTHROW/ONCLOSE for this turnout
|
||||
if (closed) onCloseLookup->handleEvent(F("CLOSE"),turnoutId);
|
||||
@@ -1334,7 +1245,6 @@ void RMFT2::thrungeString(uint32_t strfar, thrunger mode, byte id) {
|
||||
break;
|
||||
case thrunge_parse:
|
||||
case thrunge_broadcast:
|
||||
case thrunge_message:
|
||||
case thrunge_lcd:
|
||||
default: // thrunge_lcd+1, ...
|
||||
if (!buffer) buffer=new StringBuffer();
|
||||
@@ -1372,9 +1282,6 @@ void RMFT2::thrungeString(uint32_t strfar, thrunger mode, byte id) {
|
||||
case thrunge_withrottle:
|
||||
CommandDistributor::broadcastRaw(CommandDistributor::WITHROTTLE_TYPE,buffer->getString());
|
||||
break;
|
||||
case thrunge_message:
|
||||
CommandDistributor::broadcastMessage(buffer->getString());
|
||||
break;
|
||||
case thrunge_lcd:
|
||||
LCD(id,F("%s"),buffer->getString());
|
||||
break;
|
||||
|
10
EXRAIL2.h
10
EXRAIL2.h
@@ -51,10 +51,10 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
|
||||
OPCODE_JOIN,OPCODE_UNJOIN,OPCODE_READ_LOCO1,OPCODE_READ_LOCO2,
|
||||
#endif
|
||||
OPCODE_POM,
|
||||
OPCODE_START,OPCODE_SETLOCO,OPCODE_SETFREQ,OPCODE_SENDLOCO,OPCODE_FORGET,
|
||||
OPCODE_START,OPCODE_SETLOCO,OPCODE_SENDLOCO,OPCODE_FORGET,
|
||||
OPCODE_PAUSE, OPCODE_RESUME,OPCODE_POWEROFF,OPCODE_POWERON,
|
||||
OPCODE_ONCLOSE, OPCODE_ONTHROW, OPCODE_SERVOTURNOUT, OPCODE_PINTURNOUT,
|
||||
OPCODE_PRINT,OPCODE_DCCACTIVATE,OPCODE_ASPECT,
|
||||
OPCODE_PRINT,OPCODE_DCCACTIVATE,
|
||||
OPCODE_ONACTIVATE,OPCODE_ONDEACTIVATE,
|
||||
OPCODE_ROSTER,OPCODE_KILLALL,
|
||||
OPCODE_ROUTE,OPCODE_AUTOMATION,OPCODE_SEQUENCE,
|
||||
@@ -94,7 +94,7 @@ enum thrunger: byte {
|
||||
thrunge_serial,thrunge_parse,
|
||||
thrunge_serial1, thrunge_serial2, thrunge_serial3,
|
||||
thrunge_serial4, thrunge_serial5, thrunge_serial6,
|
||||
thrunge_lcn,thrunge_message,
|
||||
thrunge_lcn,
|
||||
thrunge_lcd, // Must be last!!
|
||||
};
|
||||
|
||||
@@ -155,11 +155,9 @@ class LookList {
|
||||
static void clockEvent(int16_t clocktime, bool change);
|
||||
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
|
||||
static const byte rosterNameCount;
|
||||
@@ -174,7 +172,7 @@ class LookList {
|
||||
static const FSH * getTurntableDescription(int16_t id);
|
||||
static const FSH * getTurntablePositionDescription(int16_t turntableId, uint8_t positionId);
|
||||
static void startNonRecursiveTask(const FSH* reason, int16_t id,int pc);
|
||||
|
||||
|
||||
private:
|
||||
static void ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16_t p[]);
|
||||
static bool parseSlash(Print * stream, byte & paramCount, int16_t p[]) ;
|
||||
|
@@ -31,7 +31,6 @@
|
||||
#undef ALIAS
|
||||
#undef AMBER
|
||||
#undef ANOUT
|
||||
#undef ASPECT
|
||||
#undef AT
|
||||
#undef ATGTE
|
||||
#undef ATLT
|
||||
@@ -43,9 +42,7 @@
|
||||
#undef CLEAR_STASH
|
||||
#undef CLEAR_ALL_STASH
|
||||
#undef CLOSE
|
||||
#undef CONFIGURE_SERVO
|
||||
#undef DCC_SIGNAL
|
||||
#undef DCCX_SIGNAL
|
||||
#undef DCC_TURNTABLE
|
||||
#undef DEACTIVATE
|
||||
#undef DEACTIVATEL
|
||||
@@ -87,7 +84,6 @@
|
||||
#undef IFTTPOSITION
|
||||
#undef IFRE
|
||||
#undef INVERT_DIRECTION
|
||||
#undef JMRI_SENSOR
|
||||
#undef JOIN
|
||||
#undef KILLALL
|
||||
#undef LATCH
|
||||
@@ -97,7 +93,6 @@
|
||||
#undef LCCX
|
||||
#undef LCN
|
||||
#undef MOVETT
|
||||
#undef MESSAGE
|
||||
#undef ONACTIVATE
|
||||
#undef ONACTIVATEL
|
||||
#undef ONAMBER
|
||||
@@ -157,7 +152,6 @@
|
||||
#undef SET_TRACK
|
||||
#undef SET_POWER
|
||||
#undef SETLOCO
|
||||
#undef SETFREQ
|
||||
#undef SIGNAL
|
||||
#undef SIGNALH
|
||||
#undef SPEED
|
||||
@@ -190,7 +184,6 @@
|
||||
#define AMBER(signal_id)
|
||||
#define ANOUT(vpin,value,param1,param2)
|
||||
#define AT(sensor_id)
|
||||
#define ASPECT(address,value)
|
||||
#define ATGTE(sensor_id,value)
|
||||
#define ATLT(sensor_id,value)
|
||||
#define ATTIMEOUT(sensor_id,timeout_ms)
|
||||
@@ -200,10 +193,8 @@
|
||||
#define CALL(route)
|
||||
#define CLEAR_STASH(id)
|
||||
#define CLEAR_ALL_STASH(id)
|
||||
#define CLOSE(id)
|
||||
#define CONFIGURE_SERVO(vpin,pos1,pos2,profile)
|
||||
#define CLOSE(id)
|
||||
#define DCC_SIGNAL(id,add,subaddr)
|
||||
#define DCCX_SIGNAL(id,redAspect,amberAspect,greenAspect)
|
||||
#define DCC_TURNTABLE(id,home,description)
|
||||
#define DEACTIVATE(addr,subaddr)
|
||||
#define DEACTIVATEL(addr)
|
||||
@@ -245,7 +236,6 @@
|
||||
#define IFTTPOSITION(turntable_id,position)
|
||||
#define IFRE(sensor_id,value)
|
||||
#define INVERT_DIRECTION
|
||||
#define JMRI_SENSOR(vpin,count...)
|
||||
#define JOIN
|
||||
#define KILLALL
|
||||
#define LATCH(sensor_id)
|
||||
@@ -254,7 +244,6 @@
|
||||
#define LCD(row,msg)
|
||||
#define SCREEN(display,row,msg)
|
||||
#define LCN(msg)
|
||||
#define MESSAGE(msg)
|
||||
#define MOVETT(id,steps,activity)
|
||||
#define ONACTIVATE(addr,subaddr)
|
||||
#define ONACTIVATEL(linear)
|
||||
@@ -315,7 +304,6 @@
|
||||
#define SET_TRACK(track,mode)
|
||||
#define SET_POWER(track,onoff)
|
||||
#define SETLOCO(loco)
|
||||
#define SETFREQ(loco,freq)
|
||||
#define SIGNAL(redpin,amberpin,greenpin)
|
||||
#define SIGNALH(redpin,amberpin,greenpin)
|
||||
#define SPEED(speed)
|
||||
|
@@ -51,14 +51,6 @@ void RMFT2::ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16
|
||||
opcode=0;
|
||||
break;
|
||||
|
||||
case 'A': // <A address aspect>
|
||||
if (paramCount!=2) break;
|
||||
// Ask exrail if this is just changing the aspect on a
|
||||
// predefined DCCX_SIGNAL. Because this will handle all
|
||||
// the IFRED and ONRED type issues at the same time.
|
||||
if (signalAspectEvent(p[0],p[1])) opcode=0; // all done
|
||||
break;
|
||||
|
||||
case 'L':
|
||||
// This entire code block is compiled out if LLC macros not used
|
||||
if (!(compileFeatures & FEATURE_LCC)) return;
|
||||
|
@@ -95,14 +95,14 @@ constexpr int16_t stuffSize=sizeof(compileTimeSequenceList)/sizeof(int16_t) - 1;
|
||||
|
||||
|
||||
// Compile time function to check for sequence nos.
|
||||
constexpr bool hasseq(const int16_t value, const int16_t pos=0 ) {
|
||||
constexpr bool hasseq(const int16_t value, const uint16_t pos=0 ) {
|
||||
return pos>=stuffSize? false :
|
||||
compileTimeSequenceList[pos]==value
|
||||
|| hasseq(value,pos+1);
|
||||
}
|
||||
|
||||
// Compile time function to check for duplicate sequence nos.
|
||||
constexpr bool hasdup(const int16_t value, const int16_t pos ) {
|
||||
constexpr bool hasdup(const int16_t value, const uint16_t pos ) {
|
||||
return pos>=stuffSize? false :
|
||||
compileTimeSequenceList[pos]==value
|
||||
|| hasseq(value,pos+1)
|
||||
@@ -117,9 +117,6 @@ static_assert(!hasdup(compileTimeSequenceList[0],1),"Duplicate SEQUENCE/ROUTE/AU
|
||||
// - check range on LATCH/UNLATCH
|
||||
// This pass generates no runtime data or code
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef ASPECT
|
||||
#define ASPECT(address,value) static_assert(address <=2044, "invalid Address"); \
|
||||
static_assert(address>=-3, "Invalid value");
|
||||
#undef CALL
|
||||
#define CALL(id) static_assert(hasseq(id),"Sequence not found");
|
||||
#undef FOLLOW
|
||||
@@ -152,10 +149,6 @@ static_assert(!hasdup(compileTimeSequenceList[0],1),"Duplicate SEQUENCE/ROUTE/AU
|
||||
#define HAL(haltype,params...) haltype::create(params);
|
||||
#undef HAL_IGNORE_DEFAULTS
|
||||
#define HAL_IGNORE_DEFAULTS ignore_defaults=true;
|
||||
#undef JMRI_SENSOR
|
||||
#define JMRI_SENSOR(vpin,count...) Sensor::createMultiple(vpin,##count);
|
||||
#undef CONFIGURE_SERVO
|
||||
#define CONFIGURE_SERVO(vpin,pos1,pos2,profile) IODevice::configureServo(vpin,pos1,pos2,PCA9685::profile);
|
||||
bool exrailHalSetup() {
|
||||
bool ignore_defaults=false;
|
||||
#include "myAutomation.h"
|
||||
@@ -172,8 +165,6 @@ bool exrailHalSetup() {
|
||||
#define SERVO_SIGNAL(vpin,redval,amberval,greenval) | FEATURE_SIGNAL
|
||||
#undef DCC_SIGNAL
|
||||
#define DCC_SIGNAL(id,addr,subaddr) | FEATURE_SIGNAL
|
||||
#undef DCCX_SIGNAL
|
||||
#define DCCX_SIGNAL(id,redAspect,amberAspect,greenAspect) | FEATURE_SIGNAL
|
||||
#undef VIRTUAL_SIGNAL
|
||||
#define VIRTUAL_SIGNAL(id) | FEATURE_SIGNAL
|
||||
|
||||
@@ -253,9 +244,6 @@ const int StringMacroTracker1=__COUNTER__;
|
||||
#define PRINT(msg) THRUNGE(msg,thrunge_print)
|
||||
#undef LCN
|
||||
#define LCN(msg) THRUNGE(msg,thrunge_lcn)
|
||||
#undef MESSAGE
|
||||
#define MESSAGE(msg) THRUNGE(msg,thrunge_message)
|
||||
|
||||
#undef ROUTE_CAPTION
|
||||
#define ROUTE_CAPTION(id,caption) \
|
||||
case (__COUNTER__ - StringMacroTracker1) : {\
|
||||
@@ -353,8 +341,6 @@ const FSH * RMFT2::getTurntableDescription(int16_t turntableId) {
|
||||
#define TT_ADDPOSITION(turntable_id,position,value,home,description...) T_DESC(turntable_id,position,description)
|
||||
|
||||
const FSH * RMFT2::getTurntablePositionDescription(int16_t turntableId, uint8_t positionId) {
|
||||
(void)turntableId;
|
||||
(void)positionId;
|
||||
#include "myAutomation.h"
|
||||
return NULL;
|
||||
}
|
||||
@@ -408,8 +394,6 @@ const FSH * RMFT2::getRosterFunctions(int16_t id) {
|
||||
#define SERVO_SIGNAL(vpin,redval,amberval,greenval) vpin | RMFT2::SERVO_SIGNAL_FLAG,redval,amberval,greenval,
|
||||
#undef DCC_SIGNAL
|
||||
#define DCC_SIGNAL(id,addr,subaddr) id | RMFT2::DCC_SIGNAL_FLAG,addr,subaddr,0,
|
||||
#undef DCCX_SIGNAL
|
||||
#define DCCX_SIGNAL(id,redAspect,amberAspect,greenAspect) id | RMFT2::DCCX_SIGNAL_FLAG,redAspect,amberAspect,greenAspect,
|
||||
#undef VIRTUAL_SIGNAL
|
||||
#define VIRTUAL_SIGNAL(id) id,0,0,0,
|
||||
|
||||
@@ -444,7 +428,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define ALIAS(name,value...)
|
||||
#define AMBER(signal_id) OPCODE_AMBER,V(signal_id),
|
||||
#define ANOUT(vpin,value,param1,param2) OPCODE_SERVO,V(vpin),OPCODE_PAD,V(value),OPCODE_PAD,V(param1),OPCODE_PAD,V(param2),
|
||||
#define ASPECT(address,value) OPCODE_ASPECT,V((address<<5) | (value & 0x1F)),
|
||||
#define AT(sensor_id) OPCODE_AT,V(sensor_id),
|
||||
#define ATGTE(sensor_id,value) OPCODE_ATGTE,V(sensor_id),OPCODE_PAD,V(value),
|
||||
#define ATLT(sensor_id,value) OPCODE_ATLT,V(sensor_id),OPCODE_PAD,V(value),
|
||||
@@ -456,7 +439,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define CLEAR_STASH(id) OPCODE_CLEAR_STASH,V(id),
|
||||
#define CLEAR_ALL_STASH OPCODE_CLEAR_ALL_STASH,V(0),
|
||||
#define CLOSE(id) OPCODE_CLOSE,V(id),
|
||||
#define CONFIGURE_SERVO(vpin,pos1,pos2,profile)
|
||||
#ifndef IO_NO_HAL
|
||||
#define DCC_TURNTABLE(id,home,description...) OPCODE_DCCTURNTABLE,V(id),OPCODE_PAD,V(home),
|
||||
#endif
|
||||
@@ -466,7 +448,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define DELAYMINS(mindelay) OPCODE_DELAYMINS,V(mindelay),
|
||||
#define DELAYRANDOM(mindelay,maxdelay) DELAY(mindelay) OPCODE_RANDWAIT,V((maxdelay-mindelay)/100L),
|
||||
#define DCC_SIGNAL(id,add,subaddr)
|
||||
#define DCCX_SIGNAL(id,redAspect,amberAspect,greenAspect)
|
||||
#define DONE OPCODE_ENDTASK,0,0,
|
||||
#define DRIVE(analogpin) OPCODE_DRIVE,V(analogpin),
|
||||
#define ELSE OPCODE_ELSE,0,0,
|
||||
@@ -506,7 +487,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#endif
|
||||
#define IFRE(sensor_id,value) OPCODE_IFRE,V(sensor_id),OPCODE_PAD,V(value),
|
||||
#define INVERT_DIRECTION OPCODE_INVERT_DIRECTION,0,0,
|
||||
#define JMRI_SENSOR(vpin,count...)
|
||||
#define JOIN OPCODE_JOIN,0,0,
|
||||
#define KILLALL OPCODE_KILLALL,0,0,
|
||||
#define LATCH(sensor_id) OPCODE_LATCH,V(sensor_id),
|
||||
@@ -519,7 +499,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define SCREEN(display,id,msg) PRINT(msg)
|
||||
#define STEALTH(code...) PRINT(dummy)
|
||||
#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 ONACTIVATE(addr,subaddr) OPCODE_ONACTIVATE,V(addr<<2|subaddr),
|
||||
#define ONACTIVATEL(linear) OPCODE_ONACTIVATE,V(linear+3),
|
||||
@@ -587,7 +566,6 @@ 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 SIGNAL(redpin,amberpin,greenpin)
|
||||
#define SIGNALH(redpin,amberpin,greenpin)
|
||||
#define SPEED(speed) OPCODE_SPEED,V(speed),
|
||||
|
@@ -1 +1 @@
|
||||
#define GITHUB_SHA "devel-z21-202404171118Z"
|
||||
#define GITHUB_SHA "devel-202402071454Z"
|
||||
|
@@ -83,7 +83,6 @@ void EXTurntable::_loop(unsigned long currentMicros) {
|
||||
// Read returns status as obtained in our loop.
|
||||
// Return false if our status value is invalid.
|
||||
int EXTurntable::_read(VPIN vpin) {
|
||||
(void)vpin; // surpress warning
|
||||
if (_deviceState == DEVSTATE_FAILED) return 0;
|
||||
if (_stepperStatus > 1) {
|
||||
return false;
|
||||
@@ -128,8 +127,6 @@ void EXTurntable::_writeAnalogue(VPIN vpin, int value, uint8_t activity, uint16_
|
||||
vpin, value, activity, duration);
|
||||
DIAG(F("I2CManager write I2C Address:%d stepsMSB:%d stepsLSB:%d activity:%d"),
|
||||
_I2CAddress.toString(), stepsMSB, stepsLSB, activity);
|
||||
#else
|
||||
(void)duration;
|
||||
#endif
|
||||
if (activity < 4) _stepperStatus = 1; // Tell the device driver Turntable-EX is busy
|
||||
_previousStatus = _stepperStatus;
|
||||
|
110
MotorDriver.cpp
110
MotorDriver.cpp
@@ -204,7 +204,7 @@ MotorDriver::MotorDriver(int16_t power_pin, byte signal_pin, byte signal_pin2, i
|
||||
}
|
||||
|
||||
bool MotorDriver::isPWMCapable() {
|
||||
return (!dualSignal) && DCCTimer::isPWMPin(signalPin);
|
||||
return (!dualSignal) && DCCTimer::isPWMPin(signalPin);
|
||||
}
|
||||
|
||||
|
||||
@@ -325,23 +325,49 @@ uint16_t taurustones[28] = { 165, 175, 196, 220,
|
||||
220, 196, 175, 165 };
|
||||
#endif
|
||||
#endif
|
||||
void MotorDriver::setDCSignal(byte speedcode, uint8_t frequency /*default =0*/) {
|
||||
void MotorDriver::setDCSignal(byte speedcode) {
|
||||
if (brakePin == UNUSED_PIN)
|
||||
return;
|
||||
switch(brakePin) {
|
||||
#if defined(ARDUINO_AVR_UNO)
|
||||
// Not worth doin something here as:
|
||||
// If we are on pin 9 or 10 we are on Timer1 and we can not touch Timer1 as that is our DCC source.
|
||||
// If we are on pin 5 or 6 we are on Timer 0 ad we can not touch Timer0 as that is millis() etc.
|
||||
// We are most likely not on pin 3 or 11 as no known motor shield has that as brake.
|
||||
#endif
|
||||
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
|
||||
case 9:
|
||||
case 10:
|
||||
// Timer2 (is differnet)
|
||||
TCCR2A = (TCCR2A & B11111100) | B00000001; // set WGM1=0 and WGM0=1 phase correct PWM
|
||||
TCCR2B = (TCCR2B & B11110000) | B00000110; // set WGM2=0 ; set divisor on timer 2 to 1/256 for 122.55Hz
|
||||
//DIAG(F("2 A=%x B=%x"), TCCR2A, TCCR2B);
|
||||
break;
|
||||
case 6:
|
||||
case 7:
|
||||
case 8:
|
||||
// Timer4
|
||||
TCCR4A = (TCCR4A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for normal PWM 8-bit
|
||||
TCCR4B = (TCCR4B & B11100000) | B00000100; // set WGM2=0 and WGM3=0 for normal PWM 8 bit and div 1/256 for 122.55Hz
|
||||
break;
|
||||
case 46:
|
||||
case 45:
|
||||
case 44:
|
||||
// Timer5
|
||||
TCCR5A = (TCCR5A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for normal PWM 8-bit
|
||||
TCCR5B = (TCCR5B & B11100000) | B00000100; // set WGM2=0 and WGM3=0 for normal PWM 8 bit and div 1/256 for 122.55Hz
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
// spedcoode is a dcc speed & direction
|
||||
byte tSpeed=speedcode & 0x7F; // DCC Speed with 0,1 stop and speed steps 2 to 127
|
||||
byte tDir=speedcode & 0x80;
|
||||
byte brake;
|
||||
|
||||
if (tSpeed <= 1) brake = 255;
|
||||
else if (tSpeed >= 127) brake = 0;
|
||||
else brake = 2 * (128-tSpeed);
|
||||
if (invertBrake)
|
||||
brake=255-brake;
|
||||
|
||||
{ // new block because of variable f
|
||||
#if defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_STM32)
|
||||
int f = frequency;
|
||||
{
|
||||
int f = 131;
|
||||
#ifdef VARIABLE_TONES
|
||||
if (tSpeed > 2) {
|
||||
if (tSpeed <= 58) {
|
||||
@@ -349,15 +375,19 @@ void MotorDriver::setDCSignal(byte speedcode, uint8_t frequency /*default =0*/)
|
||||
}
|
||||
}
|
||||
#endif
|
||||
//DIAG(F("Brake pin %d freqency %d"), brakePin, f);
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, f); // set DC PWM frequency
|
||||
DCCTimer::DCCEXanalogWrite(brakePin,brake);
|
||||
#else // all AVR here
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, frequency); // frequency steps
|
||||
analogWrite(brakePin,brake);
|
||||
#endif
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, f); // set DC PWM frequency to 100Hz XXX May move to setup
|
||||
}
|
||||
|
||||
#endif
|
||||
if (tSpeed <= 1) brake = 255;
|
||||
else if (tSpeed >= 127) brake = 0;
|
||||
else brake = 2 * (128-tSpeed);
|
||||
if (invertBrake)
|
||||
brake=255-brake;
|
||||
#if defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_STM32)
|
||||
DCCTimer::DCCEXanalogWrite(brakePin,brake);
|
||||
#else
|
||||
analogWrite(brakePin,brake);
|
||||
#endif
|
||||
//DIAG(F("DCSignal %d"), speedcode);
|
||||
if (HAVE_PORTA(fastSignalPin.shadowinout == &PORTA)) {
|
||||
noInterrupts();
|
||||
@@ -406,26 +436,58 @@ void MotorDriver::throttleInrush(bool on) {
|
||||
return;
|
||||
if ( !(trackMode & (TRACK_MODE_MAIN | TRACK_MODE_PROG | TRACK_MODE_EXT)))
|
||||
return;
|
||||
byte duty = on ? 207 : 0; // duty of 81% at 62500Hz this gives pauses of 3usec
|
||||
byte duty = on ? 208 : 0;
|
||||
if (invertBrake)
|
||||
duty = 255-duty;
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
if(on) {
|
||||
DCCTimer::DCCEXanalogWrite(brakePin,duty);
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 7); // 7 means max
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 62500);
|
||||
} else {
|
||||
ledcDetachPin(brakePin);
|
||||
}
|
||||
#elif defined(ARDUINO_ARCH_STM32)
|
||||
if(on) {
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 7); // 7 means max
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 62500);
|
||||
DCCTimer::DCCEXanalogWrite(brakePin,duty);
|
||||
} else {
|
||||
pinMode(brakePin, OUTPUT);
|
||||
}
|
||||
#else // all AVR here
|
||||
#else
|
||||
if(on){
|
||||
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 7); // 7 means max
|
||||
switch(brakePin) {
|
||||
#if defined(ARDUINO_AVR_UNO)
|
||||
// Not worth doin something here as:
|
||||
// If we are on pin 9 or 10 we are on Timer1 and we can not touch Timer1 as that is our DCC source.
|
||||
// If we are on pin 5 or 6 we are on Timer 0 ad we can not touch Timer0 as that is millis() etc.
|
||||
// We are most likely not on pin 3 or 11 as no known motor shield has that as brake.
|
||||
#endif
|
||||
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
|
||||
case 9:
|
||||
case 10:
|
||||
// Timer2 (is different)
|
||||
TCCR2A = (TCCR2A & B11111100) | B00000011; // set WGM0=1 and WGM1=1 for fast PWM
|
||||
TCCR2B = (TCCR2B & B11110000) | B00000001; // set WGM2=0 and prescaler div=1 (max)
|
||||
DIAG(F("2 A=%x B=%x"), TCCR2A, TCCR2B);
|
||||
break;
|
||||
case 6:
|
||||
case 7:
|
||||
case 8:
|
||||
// Timer4
|
||||
TCCR4A = (TCCR4A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for fast PWM 8-bit
|
||||
TCCR4B = (TCCR4B & B11100000) | B00001001; // set WGM2=1 and WGM3=0 for fast PWM 8 bit and div=1 (max)
|
||||
break;
|
||||
case 46:
|
||||
case 45:
|
||||
case 44:
|
||||
// Timer5
|
||||
TCCR5A = (TCCR5A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for fast PWM 8-bit
|
||||
TCCR5B = (TCCR5B & B11100000) | B00001001; // set WGM2=1 and WGM3=0 for fast PWM 8 bit and div=1 (max)
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
analogWrite(brakePin,duty);
|
||||
#endif
|
||||
|
@@ -34,15 +34,9 @@ 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,
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
TRACK_MODE_BOOST = 32,
|
||||
#else
|
||||
TRACK_MODE_BOOST = 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};
|
||||
TRACK_MODE_DC = 8, TRACK_MODE_EXT = 16, TRACK_MODE_BOOST = 32,
|
||||
TRACK_MODE_ALL = 62, // only to operate all tracks
|
||||
TRACK_MODE_INV = 64, TRACK_MODE_DCX = 72 /*DC + INV*/, TRACK_MODE_AUTOINV = 128};
|
||||
|
||||
#define setHIGH(fastpin) *fastpin.inout |= fastpin.maskHIGH
|
||||
#define setLOW(fastpin) *fastpin.inout &= fastpin.maskLOW
|
||||
@@ -193,7 +187,7 @@ class MotorDriver {
|
||||
}
|
||||
};
|
||||
inline pinpair getSignalPin() { return pinpair(signalPin,signalPin2); };
|
||||
void setDCSignal(byte speedByte, uint8_t frequency=0);
|
||||
void setDCSignal(byte speedByte);
|
||||
void throttleInrush(bool on);
|
||||
inline void detachDCSignal() {
|
||||
#if defined(__arm__)
|
||||
|
@@ -1,7 +1,7 @@
|
||||
/*
|
||||
* © 2022-2023 Paul M. Antoine
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2024 Harald Barth
|
||||
* © 2020-2023 Harald Barth
|
||||
* (c) 2020 Chris Harlow. All rights reserved.
|
||||
* (c) 2021 Fred Decker. All rights reserved.
|
||||
* (c) 2020 Harald Barth. All rights reserved.
|
||||
@@ -57,10 +57,6 @@
|
||||
// of the brake pin on the motor bridge is inverted
|
||||
// (HIGH == release brake)
|
||||
|
||||
// You can have a CS wihout any possibility to do any track signal.
|
||||
// That's strange but possible.
|
||||
#define NO_SHIELD F("No shield at all")
|
||||
|
||||
// Arduino STANDARD Motor Shield, used on different architectures:
|
||||
|
||||
#if defined(ARDUINO_ARCH_SAMD) || defined(ARDUINO_ARCH_STM32)
|
||||
|
@@ -1,39 +0,0 @@
|
||||
/*
|
||||
* © 2024 Harald Barth
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
#ifdef Z21_PROTOCOL
|
||||
#include <WiFi.h>
|
||||
#include <WiFiClient.h>
|
||||
|
||||
class NetworkClientUDP {
|
||||
public:
|
||||
NetworkClientUDP() {
|
||||
};
|
||||
bool ok() {
|
||||
return (inUse);
|
||||
};
|
||||
|
||||
bool inUse = true;
|
||||
bool connected = false;
|
||||
IPAddress remoteIP;
|
||||
int remotePort;
|
||||
|
||||
static WiFiUDP client;
|
||||
};
|
||||
#endif
|
@@ -230,13 +230,6 @@ Sensor *Sensor::create(int snum, VPIN pin, int pullUp){
|
||||
return tt;
|
||||
}
|
||||
|
||||
// Creet multiple eponymous sensors based on vpin alone.
|
||||
void Sensor::createMultiple(VPIN firstPin, byte count) {
|
||||
for (byte i=0;i<count;i++) {
|
||||
create(firstPin+i,firstPin+i,1);
|
||||
}
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
// Object method to directly change the input state, for sensors such as LCN which are updated
|
||||
// by means other than by polling an input.
|
||||
|
@@ -76,7 +76,6 @@ public:
|
||||
static void store();
|
||||
#endif
|
||||
static Sensor *create(int id, VPIN vpin, int pullUp);
|
||||
static void createMultiple(VPIN firstPin, byte count=1);
|
||||
static Sensor* get(int id);
|
||||
static bool remove(int id);
|
||||
static void checkAll();
|
||||
|
@@ -25,9 +25,6 @@ bool Diag::ACK=false;
|
||||
bool Diag::CMD=false;
|
||||
bool Diag::WIFI=false;
|
||||
bool Diag::WITHROTTLE=false;
|
||||
bool Diag::Z21THROTTLE=false;
|
||||
bool Diag::Z21THROTTLEVERBOSE=false;
|
||||
bool Diag::Z21THROTTLEDATA=false;
|
||||
bool Diag::ETHERNET=false;
|
||||
bool Diag::LCN=false;
|
||||
|
||||
|
@@ -28,9 +28,6 @@ class Diag {
|
||||
static bool CMD;
|
||||
static bool WIFI;
|
||||
static bool WITHROTTLE;
|
||||
static bool Z21THROTTLE;
|
||||
static bool Z21THROTTLEVERBOSE;
|
||||
static bool Z21THROTTLEDATA;
|
||||
static bool ETHERNET;
|
||||
static bool LCN;
|
||||
|
||||
|
@@ -1,6 +1,6 @@
|
||||
/*
|
||||
* © 2022 Chris Harlow
|
||||
* © 2022-2024 Harald Barth
|
||||
* © 2022,2023 Harald Barth
|
||||
* © 2023 Colin Murdoch
|
||||
* All rights reserved.
|
||||
*
|
||||
@@ -19,7 +19,6 @@
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include "defines.h"
|
||||
#include "TrackManager.h"
|
||||
#include "FSH.h"
|
||||
#include "DCCWaveform.h"
|
||||
@@ -41,7 +40,7 @@
|
||||
MotorDriver * TrackManager::track[MAX_TRACKS];
|
||||
int16_t TrackManager::trackDCAddr[MAX_TRACKS];
|
||||
|
||||
int8_t TrackManager::lastTrack=-1;
|
||||
byte TrackManager::lastTrack=0;
|
||||
bool TrackManager::progTrackSyncMain=false;
|
||||
bool TrackManager::progTrackBoosted=false;
|
||||
int16_t TrackManager::joinRelay=UNUSED_PIN;
|
||||
@@ -158,6 +157,12 @@ void TrackManager::setDCCSignal( bool on) {
|
||||
HAVE_PORTF(PORTF=shadowPORTF);
|
||||
}
|
||||
|
||||
void TrackManager::setCutout( bool on) {
|
||||
(void) on;
|
||||
// TODO Cutout needs fake ports as well
|
||||
// TODO APPLY_BY_MODE(TRACK_MODE_MAIN,setCutout(on));
|
||||
}
|
||||
|
||||
// setPROGSignal(), called from interrupt context
|
||||
// does assume ports are shadowed if they can be
|
||||
void TrackManager::setPROGSignal( bool on) {
|
||||
@@ -183,7 +188,7 @@ void TrackManager::setDCSignal(int16_t cab, byte speedbyte) {
|
||||
FOR_EACH_TRACK(t) {
|
||||
if (trackDCAddr[t]!=cab && cab != 0) continue;
|
||||
if (track[t]->getMode() & TRACK_MODE_DC)
|
||||
track[t]->setDCSignal(speedbyte, DCC::getThrottleFrequency(trackDCAddr[t]));
|
||||
track[t]->setDCSignal(speedbyte);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -216,13 +221,20 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
|
||||
gpio_reset_pin((gpio_num_t)p.invpin);
|
||||
}
|
||||
#ifdef BOOSTER_INPUT
|
||||
|
||||
#ifdef ARDUINO_ESP32S3_DEV
|
||||
#define LOOP_IDX SIG_IN_FUNC212_IDX //pads 208 to 212 available as loopback
|
||||
#else
|
||||
#define LOOP_IDX SIG_IN_FUNC228_IDX //pads 224 to 228 available as loopback
|
||||
#endif
|
||||
|
||||
if (mode & TRACK_MODE_BOOST) {
|
||||
//DIAG(F("Track=%c mode boost pin %d"),trackToSet+'A', p.pin);
|
||||
pinMode(BOOSTER_INPUT, INPUT);
|
||||
gpio_matrix_in(BOOSTER_INPUT, SIG_IN_FUNC228_IDX, false); //pads 224 to 228 available as loopback
|
||||
gpio_matrix_out(p.pin, SIG_IN_FUNC228_IDX, false, false);
|
||||
gpio_matrix_in(26, LOOP_IDX, false);
|
||||
gpio_matrix_out(p.pin, LOOP_IDX, false, false);
|
||||
if (p.invpin != UNUSED_PIN) {
|
||||
gpio_matrix_out(p.invpin, SIG_IN_FUNC228_IDX, true /*inverted*/, false);
|
||||
gpio_matrix_out(p.invpin, LOOP_IDX, true /*inverted*/, false);
|
||||
}
|
||||
} else // elseif clause continues
|
||||
#endif
|
||||
@@ -329,8 +341,8 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
|
||||
}
|
||||
|
||||
void TrackManager::applyDCSpeed(byte t) {
|
||||
track[t]->setDCSignal(DCC::getThrottleSpeedByte(trackDCAddr[t]),
|
||||
DCC::getThrottleFrequency(trackDCAddr[t]));
|
||||
uint8_t speedByte=DCC::getThrottleSpeedByte(trackDCAddr[t]);
|
||||
track[t]->setDCSignal(speedByte);
|
||||
}
|
||||
|
||||
bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])
|
||||
@@ -362,8 +374,7 @@ bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])
|
||||
if (params==2 && p[1]=="EXT"_hk) // <= id EXT>
|
||||
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>
|
||||
if (params==2 && p[1]=="BOOST"_hk) // <= id BOOST>
|
||||
return setTrackMode(p[0],TRACK_MODE_BOOST);
|
||||
#endif
|
||||
if (params==2 && p[1]=="AUTO"_hk) // <= id AUTO>
|
||||
@@ -402,11 +413,11 @@ const FSH* TrackManager::getModeName(TRACK_MODE tm) {
|
||||
modename=F("EXT");
|
||||
else if(tm & TRACK_MODE_BOOST) {
|
||||
if(tm & TRACK_MODE_AUTOINV)
|
||||
modename=F("BOOST A");
|
||||
modename=F("B A");
|
||||
else if (tm & TRACK_MODE_INV)
|
||||
modename=F("BOOST I");
|
||||
modename=F("B I");
|
||||
else
|
||||
modename=F("BOOST");
|
||||
modename=F("B");
|
||||
}
|
||||
else if (tm & TRACK_MODE_DC) {
|
||||
if (tm & TRACK_MODE_INV)
|
||||
@@ -498,11 +509,7 @@ void TrackManager::setTrackPower(TRACK_MODE trackmodeToMatch, POWERMODE powermod
|
||||
|
||||
// Set track power for this track, inependent of mode
|
||||
void TrackManager::setTrackPower(POWERMODE powermode, byte t) {
|
||||
MotorDriver *driver=track[t];
|
||||
if (driver == NULL) { // track is not defined at all
|
||||
DIAG(F("Error: Track %c does not exist"), t+'A');
|
||||
return;
|
||||
}
|
||||
MotorDriver *driver=track[t];
|
||||
TRACK_MODE trackmode = driver->getMode();
|
||||
POWERMODE oldpower = driver->getPower();
|
||||
if (trackmode & TRACK_MODE_NONE) {
|
||||
@@ -560,17 +567,14 @@ bool TrackManager::getPower(byte t, char s[]) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void TrackManager::reportObsoleteCurrent(Print* stream) {
|
||||
// This function is for backward JMRI compatibility only
|
||||
// It reports the first track only, as main, regardless of track settings.
|
||||
// <c MeterName value C/V unit min max res warn>
|
||||
#ifdef HAS_ENOUGH_MEMORY
|
||||
int maxCurrent=track[0]->raw2mA(track[0]->getRawCurrentTripValue());
|
||||
StringFormatter::send(stream, F("<c CurrentMAIN %d C Milli 0 %d 1 %d>\n"),
|
||||
track[0]->raw2mA(track[0]->getCurrentRaw(false)), maxCurrent, maxCurrent);
|
||||
#else
|
||||
(void)stream;
|
||||
#endif
|
||||
track[0]->raw2mA(track[0]->getCurrentRaw(false)), maxCurrent, maxCurrent);
|
||||
}
|
||||
|
||||
void TrackManager::reportCurrent(Print* stream) {
|
||||
|
@@ -1,6 +1,6 @@
|
||||
/*
|
||||
* © 2022 Chris Harlow
|
||||
* © 2022-2024 Harald Barth
|
||||
* © 2022 Harald Barth
|
||||
* © 2023 Colin Murdoch
|
||||
*
|
||||
* All rights reserved.
|
||||
@@ -46,7 +46,7 @@ const byte TRACK_POWER_1=1, TRACK_POWER_ON=1;
|
||||
class TrackManager {
|
||||
public:
|
||||
static void Setup(const FSH * shieldName,
|
||||
MotorDriver * track0=NULL,
|
||||
MotorDriver * track0,
|
||||
MotorDriver * track1=NULL,
|
||||
MotorDriver * track2=NULL,
|
||||
MotorDriver * track3=NULL,
|
||||
@@ -57,6 +57,7 @@ class TrackManager {
|
||||
);
|
||||
|
||||
static void setDCCSignal( bool on);
|
||||
static void setCutout( bool on);
|
||||
static void setPROGSignal( bool on);
|
||||
static void setDCSignal(int16_t cab, byte speedbyte);
|
||||
static MotorDriver * getProgDriver();
|
||||
@@ -108,7 +109,7 @@ class TrackManager {
|
||||
|
||||
private:
|
||||
static void addTrack(byte t, MotorDriver* driver);
|
||||
static int8_t lastTrack;
|
||||
static byte lastTrack;
|
||||
static byte nextCycleTrack;
|
||||
static void applyDCSpeed(byte t);
|
||||
|
||||
|
@@ -123,6 +123,7 @@
|
||||
return true;
|
||||
}
|
||||
|
||||
#define DIAG_IO
|
||||
// Static setClosed function is invoked from close(), throw() etc. to perform the
|
||||
// common parts of the turnout operation. Code which is specific to a turnout
|
||||
// type should be placed in the virtual function setClosedInternal(bool) which is
|
||||
|
@@ -247,23 +247,22 @@ DCCTurntable::DCCTurntable(uint16_t id) : Turntable(id, TURNTABLE_DCC) {}
|
||||
StringFormatter::send(stream, F("<i %d DCCTURNTABLE>\n"), _turntableData.id);
|
||||
}
|
||||
|
||||
// EX-Turntable specific code for moving to the specified position
|
||||
bool DCCTurntable::setPositionInternal(uint8_t position, uint8_t activity) {
|
||||
(void) activity;
|
||||
// EX-Turntable specific code for moving to the specified position
|
||||
bool DCCTurntable::setPositionInternal(uint8_t position, uint8_t activity) {
|
||||
#ifndef IO_NO_HAL
|
||||
int16_t value = getPositionValue(position);
|
||||
if (position == 0 || !value) return false; // Return false if it's not a valid position
|
||||
// Set position via device driver
|
||||
int16_t addr=value>>3;
|
||||
int16_t subaddr=(value>>1) & 0x03;
|
||||
bool active=value & 0x01;
|
||||
_previousPosition = _turntableData.position;
|
||||
_turntableData.position = position;
|
||||
DCC::setAccessory(addr, subaddr, active);
|
||||
int16_t value = getPositionValue(position);
|
||||
if (position == 0 || !value) return false; // Return false if it's not a valid position
|
||||
// Set position via device driver
|
||||
int16_t addr=value>>3;
|
||||
int16_t subaddr=(value>>1) & 0x03;
|
||||
bool active=value & 0x01;
|
||||
_previousPosition = _turntableData.position;
|
||||
_turntableData.position = position;
|
||||
DCC::setAccessory(addr, subaddr, active);
|
||||
#else
|
||||
(void)position;
|
||||
(void)position;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
@@ -571,7 +571,7 @@ void WiThrottle::sendRoutes(Print* stream) {
|
||||
|
||||
void WiThrottle::sendFunctions(Print* stream, byte loco) {
|
||||
int16_t locoid=myLocos[loco].cab;
|
||||
int fkeys=32; // upper limit (send functions 0 to 31)
|
||||
int fkeys=29;
|
||||
myLocos[loco].functionToggles=1<<2; // F2 (HORN) is a non-toggle
|
||||
|
||||
#ifdef EXRAIL_ACTIVE
|
||||
@@ -621,7 +621,7 @@ void WiThrottle::sendFunctions(Print* stream, byte loco) {
|
||||
#endif
|
||||
|
||||
for(int fKey=0; fKey<fkeys; fKey++) {
|
||||
int8_t fstate=DCC::getFn(locoid,fKey);
|
||||
int fstate=DCC::getFn(locoid,fKey);
|
||||
if (fstate>=0) StringFormatter::send(stream,F("M%cA%c%d<;>F%d%d\n"),myLocos[loco].throttle,LorS(locoid),locoid,fstate,fKey);
|
||||
}
|
||||
}
|
||||
|
@@ -30,10 +30,6 @@
|
||||
#include "RingStream.h"
|
||||
#include "CommandDistributor.h"
|
||||
#include "WiThrottle.h"
|
||||
|
||||
#ifdef Z21_PROTOCOL
|
||||
#include "Z21Throttle.h"
|
||||
#endif
|
||||
/*
|
||||
#include "soc/rtc_wdt.h"
|
||||
#include "esp_task_wdt.h"
|
||||
@@ -43,9 +39,15 @@
|
||||
#include "soc/timer_group_reg.h"
|
||||
void feedTheDog0(){
|
||||
// feed dog 0
|
||||
#ifdef ARDUINO_ESP32S3_DEV
|
||||
TIMERG0.wdtwprotect.wdt_wkey=0x50D83AA1; //MWDT_LL_WKEY_VALUE? write enable
|
||||
TIMERG0.wdtfeed.wdt_feed=1; // feed dog
|
||||
TIMERG0.wdtwprotect.wdt_wkey=0; // write protect
|
||||
#else
|
||||
TIMERG0.wdt_wprotect=TIMG_WDT_WKEY_VALUE; // write enable
|
||||
TIMERG0.wdt_feed=1; // feed dog
|
||||
TIMERG0.wdt_wprotect=0; // write protect
|
||||
#endif
|
||||
// feed dog 1
|
||||
//TIMERG1.wdt_wprotect=TIMG_WDT_WKEY_VALUE; // write enable
|
||||
//TIMERG1.wdt_feed=1; // feed dog
|
||||
@@ -141,7 +143,6 @@ bool WifiESP::setup(const char *SSid,
|
||||
bool havePassword = true;
|
||||
bool haveSSID = true;
|
||||
bool wifiUp = false;
|
||||
IPAddress localIP;
|
||||
uint8_t tries = 40;
|
||||
|
||||
//#ifdef SERIAL_BT_COMMANDS
|
||||
@@ -200,7 +201,7 @@ bool WifiESP::setup(const char *SSid,
|
||||
delay(500);
|
||||
}
|
||||
if (WiFi.status() == WL_CONNECTED) {
|
||||
DIAG(F("Wifi STA IP 2nd try %s"),(localIP=WiFi.localIP()).toString().c_str());
|
||||
DIAG(F("Wifi STA IP 2nd try %s"),WiFi.localIP().toString().c_str());
|
||||
wifiUp = true;
|
||||
} else {
|
||||
DIAG(F("Wifi STA mode FAIL. Will revert to AP mode"));
|
||||
@@ -261,13 +262,9 @@ bool WifiESP::setup(const char *SSid,
|
||||
DIAG(F("Wifi setup failed to add withrottle service to mDNS"));
|
||||
}
|
||||
|
||||
// server for WiThrottle and DCCEX protocol started here
|
||||
server = new WiFiServer(port); // start listening on tcp port
|
||||
server->begin();
|
||||
#ifdef Z21_PROTOCOL
|
||||
// server for Z21 Protocol started here
|
||||
Z21Throttle::setup(localIP, Z21_UDPPORT);
|
||||
#endif
|
||||
// server started here
|
||||
|
||||
#ifdef WIFI_TASK_ON_CORE0
|
||||
//start loop task
|
||||
@@ -342,9 +339,6 @@ void WifiESP::loop() {
|
||||
} // all clients
|
||||
|
||||
WiThrottle::loop(outboundRing);
|
||||
#ifdef Z21_PROTOCOL
|
||||
Z21Throttle::loop();
|
||||
#endif
|
||||
|
||||
// something to write out?
|
||||
clientId=outboundRing->read();
|
||||
|
1001
Z21Throttle.cpp
1001
Z21Throttle.cpp
File diff suppressed because it is too large
Load Diff
224
Z21Throttle.h
224
Z21Throttle.h
@@ -1,224 +0,0 @@
|
||||
/*
|
||||
* © 2023 Thierry Paris / Locoduino
|
||||
* © 2023 Harald Barth
|
||||
* All rights reserved.
|
||||
*
|
||||
* This is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* It is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#ifndef Z21Throttle_h
|
||||
#define Z21Throttle_h
|
||||
|
||||
#include "defines.h"
|
||||
#ifdef Z21_PROTOCOL
|
||||
|
||||
//#include "CircularBuffer.hpp"
|
||||
#include "NetworkClientUDP.h"
|
||||
|
||||
#define MAX_MTU 1460
|
||||
#define UDPBYTE_SIZE 1500
|
||||
#define UDP_BUFFERSIZE 2048
|
||||
|
||||
// Broadcast flags
|
||||
|
||||
#define BROADCAST_BASE 0x00000001
|
||||
#define BROADCAST_RBUS 0x00000002
|
||||
#define BROADCAST_RAILCOM 0x00000004
|
||||
#define BROADCAST_SYSTEM 0x00000100
|
||||
#define BROADCAST_BASE_LOCOINFO 0x00010000
|
||||
#define BROADCAST_LOCONET 0x01000000
|
||||
#define BROADCAST_LOCONET_LOCO 0x02000000
|
||||
#define BROADCAST_LOCONET_SWITCH 0x04000000
|
||||
#define BROADCAST_LOCONET_DETECTOR 0x08000000
|
||||
#define BROADCAST_RAILCOM_AUTO 0x00040000
|
||||
#define BROADCAST_CAN 0x00080000
|
||||
|
||||
struct MYLOCOZ21 {
|
||||
char throttle; //indicates which throttle letter on client, '0' + clientid
|
||||
int cab; //address of this loco
|
||||
bool broadcastPending;
|
||||
uint32_t functionMap;
|
||||
uint32_t functionToggles;
|
||||
};
|
||||
|
||||
class Z21Throttle {
|
||||
public:
|
||||
static void loop();
|
||||
static Z21Throttle* getOrAddThrottle(int clientId);
|
||||
static void broadcastNotifyTurnout(uint16_t addr, bool isClosed);
|
||||
static void broadcastNotifySensor(uint16_t addr, bool state);
|
||||
static void markForBroadcast(int cab);
|
||||
static void forget(byte clientId);
|
||||
static void findUniqThrottle(int id, char *u);
|
||||
static void setup(IPAddress ip, int port);
|
||||
|
||||
void notifyCvNACK(int inCvAddress);
|
||||
void notifyCvRead(int inCvAddress, int inValue);
|
||||
|
||||
bool parse(byte *networkPacket, int len);
|
||||
|
||||
static Z21Throttle *readWriteThrottle; // NULL if no throttle is reading or writing a CV...
|
||||
static int cvAddress;
|
||||
static int cvValue;
|
||||
|
||||
private:
|
||||
Z21Throttle(int clientId);
|
||||
~Z21Throttle();
|
||||
|
||||
static const int MAX_MY_LOCO=10; // maximum number of locos assigned to a single client
|
||||
static const int HEARTBEAT_SECONDS=10; // heartbeat at 4secs to provide messaging transport
|
||||
static const int ESTOP_SECONDS=20; // eStop if no incoming messages for more than 8secs
|
||||
static Z21Throttle* firstThrottle;
|
||||
static byte commBuffer[100];
|
||||
static byte replyBuffer[20];
|
||||
static char LorS(int cab);
|
||||
static bool isThrottleInUse(int cab);
|
||||
static void setSendTurnoutList();
|
||||
bool areYouUsingThrottle(int cab);
|
||||
Z21Throttle* nextThrottle;
|
||||
|
||||
int clientid;
|
||||
char uniq[17] = "";
|
||||
|
||||
MYLOCOZ21 myLocos[MAX_MY_LOCO];
|
||||
|
||||
int CountLocos() {
|
||||
int count = 0;
|
||||
for (int loco=0;loco<MAX_MY_LOCO;loco++)
|
||||
if (myLocos[loco].throttle != '\0')
|
||||
count++;
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
bool initSent; // valid connection established
|
||||
bool exRailSent; // valid connection established
|
||||
uint16_t mostRecentCab;
|
||||
int turnoutListHash; // used to check for changes to turnout list
|
||||
bool lastPowerState; // last power state sent to this client
|
||||
int32_t broadcastFlags = BROADCAST_BASE;
|
||||
|
||||
int getOrAddLoco(int cab);
|
||||
void printLocomotives(bool addTab = false);
|
||||
static void printThrottles(bool printLocomotives);
|
||||
|
||||
// sizes : [ 2 ][ 2 ][inLengthData]
|
||||
// bytes : [length1, length2][Header1, Header2][Data........]
|
||||
bool notify(unsigned int inHeader, byte* inpData, unsigned int inLengthData, bool inXorInData);
|
||||
|
||||
// sizes : [ 2 ][ 2 ][ 1 ][inLengthData]
|
||||
// bytes : [length1, length2][Header1, Header2][XHeader][Data........]
|
||||
bool notify(unsigned int inHeader, unsigned int inXHeader, byte* inpData, unsigned int inLengthData, bool inXorInData);
|
||||
|
||||
// sizes : [ 2 ][ 2 ][ 1 ][ 1 ][inLengthData]
|
||||
// bytes : [length1, length2][Header1, Header2][XHeader][DB0][Data........]
|
||||
bool notify(unsigned int inHeader, unsigned int inXHeader, byte inDB0, byte* inpData, unsigned int inLengthData, bool inXorInData);
|
||||
|
||||
void notifyStatus();
|
||||
void notifyLocoInfo(byte inMSB, byte inLSB);
|
||||
void notifyTurnoutInfo(uint16_t addr, bool isClosed);
|
||||
void notifyTurnoutInfo(byte inMSB, byte inLSB);
|
||||
void notifyTurnoutInfo(byte inMSB, byte inLSB, bool isClosed);
|
||||
void notifySensor(uint16_t addr);
|
||||
void notifySensor(uint16_t addr, bool state);
|
||||
void notifyLocoMode(byte inMSB, byte inLSB);
|
||||
void notifyFirmwareVersion();
|
||||
void notifyHWInfo();
|
||||
void write(byte* inpData, int inLengthData);
|
||||
|
||||
void setSpeed(byte inNbSteps, byte inDB1, byte inDB2, byte inDB3);
|
||||
void setFunction(byte inDB1, byte inDB2, byte inDB3);
|
||||
void setTurnout(byte addrMSB, byte addrLSB, byte command);
|
||||
void cvReadProg(byte inDB1, byte inDB2);
|
||||
void cvWriteProg(byte inDB1, byte inDB2, byte inDB3);
|
||||
void cvReadMain(byte inDB1, byte inDB2);
|
||||
void cvReadPom(byte inDB1, byte inDB2, byte inDB3, byte inDB4);
|
||||
|
||||
// callback stuff to support prog track acquire
|
||||
static Z21Throttle * stashInstance;
|
||||
static byte stashClient;
|
||||
static char stashThrottleChar;
|
||||
static void getLocoCallback(int16_t locoid);
|
||||
};
|
||||
|
||||
#define Z21_UDPPORT 21105
|
||||
#define Z21_TIMEOUT 60000 // if no activity during 1 minute, disconnect the throttle...
|
||||
|
||||
#define HEADER_LAN_GET_SERIAL_NUMBER 0x10
|
||||
#define HEADER_LAN_LOGOFF 0x30
|
||||
#define HEADER_LAN_XPRESS_NET 0x40
|
||||
#define HEADER_LAN_SET_BROADCASTFLAGS 0x50
|
||||
#define HEADER_LAN_GET_BROADCASTFLAGS 0x51
|
||||
#define HEADER_LAN_SYSTEMSTATE_GETDATA 0x85 //0x141 0x21 0x24 0x05
|
||||
#define HEADER_LAN_GET_HWINFO 0x1A
|
||||
#define HEADER_LAN_GET_LOCOMODE 0x60
|
||||
#define HEADER_LAN_SET_LOCOMODE 0x61
|
||||
#define HEADER_LAN_GET_TURNOUTMODE 0x70
|
||||
#define HEADER_LAN_SET_TURNOUTMODE 0x71
|
||||
#define HEADER_LAN_RMBUS_DATACHANGED 0x80
|
||||
#define HEADER_LAN_RMBUS_GETDATA 0x81
|
||||
#define HEADER_LAN_RMBUS_PROGRAMMODULE 0x82
|
||||
#define HEADER_LAN_RAILCOM_DATACHANGED 0x88
|
||||
#define HEADER_LAN_RAILCOM_GETDATA 0x89
|
||||
#define HEADER_LAN_LOCONET_DISPATCH_ADDR 0xA3
|
||||
#define HEADER_LAN_LOCONET_DETECTOR 0xA4
|
||||
|
||||
#define LAN_GET_CONFIG 0x12
|
||||
|
||||
#define LAN_LOCONET_TYPE_DIGITRAX 0x80
|
||||
#define LAN_LOCONET_TYPE_UHL_REPORTER 0x81
|
||||
#define LAN_LOCONET_TYPE_UHL_LISSY 0x82
|
||||
|
||||
#define LAN_X_HEADER_GENERAL 0x21
|
||||
#define LAN_X_HEADER_READ_REGISTER 0x22
|
||||
#define LAN_X_HEADER_SET_STOP 0x80
|
||||
#define LAN_X_HEADER_GET_FIRMWARE_VERSION 0xF1 //0x141 0x21 0x21 0x00
|
||||
#define LAN_X_HEADER_GET_LOCO_INFO 0xE3
|
||||
#define LAN_X_HEADER_SET_LOCO 0xE4
|
||||
#define LAN_X_HEADER_GET_TURNOUT_INFO 0x43
|
||||
#define LAN_X_HEADER_SET_TURNOUT 0x53
|
||||
#define LAN_X_HEADER_CV_READ 0x23
|
||||
#define LAN_X_HEADER_CV_WRITE 0x24
|
||||
#define LAN_X_HEADER_CV_POM 0xE6
|
||||
|
||||
#define LAN_X_DB0_READ_REGISTER 0x11
|
||||
#define LAN_X_DB0_GET_VERSION 0x21
|
||||
#define LAN_X_DB0_GET_STATUS 0x24
|
||||
#define LAN_X_DB0_SET_TRACK_POWER_OFF 0x80
|
||||
#define LAN_X_DB0_SET_TRACK_POWER_ON 0x81
|
||||
#define LAN_X_DB0_LOCO_DCC14 0x10
|
||||
#define LAN_X_DB0_LOCO_DCC28 0x12
|
||||
#define LAN_X_DB0_LOCO_DCC128 0x13
|
||||
#define LAN_X_DB0_SET_LOCO_FUNCTION 0xF8
|
||||
#define LAN_X_DB0_CV_POM_WRITE 0x30
|
||||
#define LAN_X_DB0_CV_POM_ACCESSORY_WRITE 0x31
|
||||
|
||||
#define LAN_X_OPTION_CV_POM_WRITE_BYTE 0xEC
|
||||
#define LAN_X_OPTION_CV_POM_WRITE_BIT 0xE8
|
||||
#define LAN_X_OPTION_CV_POM_READ_BYTE 0xE4
|
||||
|
||||
// Replies to the controllers
|
||||
#define HEADER_LAN_SYSTEMSTATE 0x84
|
||||
|
||||
#define LAN_X_HEADER_LOCO_INFO 0xEF
|
||||
#define LAN_X_HEADER_TURNOUT_INFO 0x43
|
||||
#define LAN_X_HEADER_FIRMWARE_VERSION 0xF3
|
||||
#define LAN_X_HEADER_CV_NACK 0x61
|
||||
#define LAN_X_HEADER_CV_RESULT 0x64
|
||||
|
||||
#define LAN_X_DB0_CV_NACK_SC 0x12
|
||||
#define LAN_X_DB0_CV_NACK 0x13
|
||||
|
||||
|
||||
#endif // Z21PROTOCOL
|
||||
#endif
|
@@ -300,12 +300,6 @@ The configuration file for DCC-EX Command Station
|
||||
//
|
||||
//#define BOOSTER_INPUT 26
|
||||
|
||||
// Z21 protocol support
|
||||
// On ESP32 you have the possibility to use the Z21 app
|
||||
// and other devices that use the Z21 protocol over IP/UDP.
|
||||
//
|
||||
//#define Z21_PROTOCOL
|
||||
|
||||
// SABERTOOTH
|
||||
//
|
||||
// This is a very special option and only useful if you happen to have a
|
||||
|
@@ -157,14 +157,6 @@
|
||||
#define CPU_TYPE_ERROR
|
||||
#endif
|
||||
|
||||
// Only ESP32 does support the Z21 protocol currently
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
#ifdef Z21_PROTOCOL
|
||||
#warning "Z21 protocol can not be used on this platform, disabling"
|
||||
#undef Z21_PROTOCOL
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// replace board type if provided by compiler
|
||||
#ifdef BOARD_NAME
|
||||
#undef ARDUINO_TYPE
|
||||
|
@@ -148,7 +148,10 @@ build_flags =
|
||||
platform = atmelavr
|
||||
board = uno
|
||||
framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
lib_deps =
|
||||
${env.lib_deps}
|
||||
arduino-libraries/Ethernet
|
||||
SPI
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
build_flags = -mcall-prologues
|
||||
@@ -161,7 +164,6 @@ framework = arduino
|
||||
lib_deps = ${env.lib_deps}
|
||||
monitor_speed = 115200
|
||||
monitor_echo = yes
|
||||
build_flags = -mcall-prologues
|
||||
|
||||
[env:ESP32]
|
||||
platform = espressif32
|
||||
|
26
version.h
26
version.h
@@ -3,30 +3,8 @@
|
||||
|
||||
#include "StringFormatter.h"
|
||||
|
||||
#define VERSION "5.99.99"
|
||||
// Make Z21 diag selectable from DCCEXParser
|
||||
// Z21 error fix for turnout feedback
|
||||
// Z21 add turnout and sensor broadcasts
|
||||
// Z21: report the (unchanged) loco mode back after every attempted change
|
||||
// Add turnout functionality to Z21
|
||||
// Fix index of DB[] to comply with Roco documentation
|
||||
// add READ_REGISTER name to Z21 parser
|
||||
//
|
||||
// 5.2.40 - Allow no shield
|
||||
// 5.2.39 - Functions for DC frequency: Use func up to F31
|
||||
// 5.2.38 - Exrail MESSAGE("text") to send a user message to all
|
||||
// connected throttles (uses <m "text"> and withrottle Hmtext.
|
||||
// 5.2.37 - Bugfix ESP32: Use BOOSTER_INPUT define
|
||||
// 5.2.36 - Variable frequency for DC mode
|
||||
// 5.2.35 - Bugfix: Make DCC Extended Accessories follow RCN-213
|
||||
// 5.2.34 - <A address aspect> Command fopr DCC Extended Accessories
|
||||
// - Exrail ASPECT(address,aspect) for above.
|
||||
// - EXRAIL DCCX_SIGNAL(Address,redAspect,amberAspect,greenAspect)
|
||||
// - Exrail intercept <A ...> for DCC Signals.
|
||||
// 5.2.33 - Exrail CONFIGURE_SERVO(vpin,pos1,pos2,profile)
|
||||
// 5.2.32 - Railcom Cutout (Initial trial Mega2560 only)
|
||||
// 5.2.31 - Exrail JMRI_SENSOR(vpin [,count]) creates <S> types.
|
||||
// 5.2.30 - Bugfix: WiThrottle sendIntro after initial N message as well
|
||||
#define VERSION "5.2.30"
|
||||
// 5.2.40 - Bugfix: WiThrottle sendIntro after initial N message as well
|
||||
// 5.2.29 - Added IO_I2CDFPlayer.h to support DFPLayer over I2C connected to NXP SC16IS750/SC16IS752 (currently only single UART for SC16IS752)
|
||||
// - Added enhanced IO_I2CDFPLayer enum commands to EXRAIL2.h
|
||||
// - Added PLAYSOUND alias of ANOUT to EXRAILMacros.h
|
||||
|
Reference in New Issue
Block a user