mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2025-07-30 19:03:44 +02:00
Compare commits
64 Commits
v5.2.27-De
...
1536a46474
Author | SHA1 | Date | |
---|---|---|---|
|
1536a46474 | ||
|
8a813c2a1e | ||
|
050fed6e9a | ||
|
741771c4fe | ||
|
b632088b19 | ||
|
d95d9c193e | ||
|
05db1bdd90 | ||
|
b1f5c34ef2 | ||
|
701bc0a837 | ||
|
dfa798c149 | ||
|
d46a6f092a | ||
|
d0759e97fd | ||
|
22323ea065 | ||
|
13bb1175f3 | ||
|
2f72a3dd57 | ||
|
141b46f09e | ||
|
10a4d1632a | ||
|
b81b7ee27f | ||
|
d52f422f05 | ||
|
6ec16c94d7 | ||
|
a7a1daf5b4 | ||
|
b98ddf7886 | ||
|
f2c9b5a496 | ||
|
88f1c0c580 | ||
|
8bbe30e789 | ||
|
84b6207988 | ||
|
80365c214e | ||
|
e54cd0919c | ||
|
0a63befee9 | ||
|
bbca4379d2 | ||
|
dff7eb37ab | ||
|
2edc223beb | ||
|
38f4d5f9d9 | ||
|
decebd1802 | ||
|
f1aace96d5 | ||
|
169050853a | ||
|
4bc7c2632a | ||
|
1745fa72db | ||
|
f88c617dbe | ||
|
70c1f1db2a | ||
|
e816ef2b03 | ||
|
a84eba7ab6 | ||
|
be0471679d | ||
|
706076e4f1 | ||
|
7259924466 | ||
|
917fb569ba | ||
|
809388c547 | ||
|
3d4b80d02e | ||
|
881c490ae0 | ||
|
1d6b7d4c63 | ||
|
9cff33a414 | ||
|
6ea3366c75 | ||
|
e5ffab582f | ||
|
2993725a14 | ||
|
b417ad6575 | ||
|
94273b6e11 | ||
|
5b4a1dd6fa | ||
|
9e6104fc16 | ||
|
5955a9f593 | ||
|
bcf8e27e43 | ||
|
c8e46fed76 | ||
|
23a0a42df2 | ||
|
2a03b08d28 | ||
|
ef78b52c2a |
@@ -105,7 +105,6 @@ void CommandDistributor::parse(byte clientId,byte * buffer, RingStream * stream
|
||||
void CommandDistributor::forget(byte clientId) {
|
||||
if (clients[clientId]==WITHROTTLE_TYPE) WiThrottle::forget(clientId);
|
||||
clients[clientId]=NONE_TYPE;
|
||||
if (virtualLCDClient==clientId) virtualLCDClient=RingStream::NO_CLIENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -249,123 +248,27 @@ void CommandDistributor::broadcastLoco(byte slot) {
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastPower() {
|
||||
char pstr[] = "? x";
|
||||
for(byte t=0; t<TrackManager::MAX_TRACKS; t++)
|
||||
if (TrackManager::getPower(t, pstr))
|
||||
broadcastReply(COMMAND_TYPE, F("<p%s>\n"),pstr);
|
||||
|
||||
byte trackcount=0;
|
||||
byte oncount=0;
|
||||
byte offcount=0;
|
||||
for(byte t=0; t<TrackManager::MAX_TRACKS; t++) {
|
||||
if (TrackManager::isActive(t)) {
|
||||
trackcount++;
|
||||
// do not call getPower(t) unless isActive(t)!
|
||||
if (TrackManager::getPower(t) == POWERMODE::ON)
|
||||
oncount++;
|
||||
else
|
||||
offcount++;
|
||||
}
|
||||
}
|
||||
//DIAG(F("t=%d on=%d off=%d"), trackcount, oncount, offcount);
|
||||
|
||||
char state='2';
|
||||
if (oncount==0 || offcount == trackcount)
|
||||
state = '0';
|
||||
else if (oncount == trackcount) {
|
||||
state = '1';
|
||||
}
|
||||
|
||||
// additional info about MAIN, PROG and JOIN
|
||||
bool main=TrackManager::getMainPower()==POWERMODE::ON;
|
||||
bool prog=TrackManager::getProgPower()==POWERMODE::ON;
|
||||
bool join=TrackManager::isJoined();
|
||||
//DIAG(F("m=%d p=%d j=%d"), main, prog, join);
|
||||
const FSH * reason=F("");
|
||||
if (join) {
|
||||
reason = F(" JOIN"); // with space at start so we can append without space
|
||||
broadcastReply(COMMAND_TYPE, F("<p1 %S>\n"),reason);
|
||||
} else {
|
||||
if (main) {
|
||||
//reason = F("MAIN");
|
||||
broadcastReply(COMMAND_TYPE, F("<p1 MAIN>\n"));
|
||||
}
|
||||
if (prog) {
|
||||
//reason = F("PROG");
|
||||
broadcastReply(COMMAND_TYPE, F("<p1 PROG>\n"));
|
||||
}
|
||||
}
|
||||
|
||||
if (state != '2')
|
||||
broadcastReply(COMMAND_TYPE, F("<p%c>\n"),state);
|
||||
char state='1';
|
||||
if (main && prog && join) reason=F(" JOIN");
|
||||
else if (main && prog);
|
||||
else if (main) reason=F(" MAIN");
|
||||
else if (prog) reason=F(" PROG");
|
||||
else state='0';
|
||||
broadcastReply(COMMAND_TYPE, F("<p%c%S>\n"),state,reason);
|
||||
#ifdef CD_HANDLE_RING
|
||||
// send '1' if all main are on, otherwise global state (which in that case is '0' or '2')
|
||||
broadcastReply(WITHROTTLE_TYPE, F("PPA%c\n"), main?'1': state);
|
||||
broadcastReply(WITHROTTLE_TYPE, F("PPA%c\n"), main?'1':'0');
|
||||
#endif
|
||||
|
||||
LCD(2,F("Power %S%S"),state=='1'?F("On"): ( state=='0'? F("Off") : F("SC") ),reason);
|
||||
LCD(2,F("Power %S%S"),state=='1'?F("On"):F("Off"),reason);
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastRaw(clientType type, char * msg) {
|
||||
broadcastReply(type, F("%s"),msg);
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastTrackState(const FSH* format, byte trackLetter, const FSH *modename, int16_t dcAddr) {
|
||||
broadcastReply(COMMAND_TYPE, format, trackLetter, modename, dcAddr);
|
||||
void CommandDistributor::broadcastTrackState(const FSH* format,byte trackLetter, int16_t dcAddr) {
|
||||
broadcastReply(COMMAND_TYPE, format,trackLetter, dcAddr);
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastRouteState(uint16_t routeId, byte state ) {
|
||||
broadcastReply(COMMAND_TYPE, F("<jB %d %d>\n"),routeId,state);
|
||||
}
|
||||
|
||||
void CommandDistributor::broadcastRouteCaption(uint16_t routeId, const FSH* caption ) {
|
||||
broadcastReply(COMMAND_TYPE, F("<jB %d \"%S\">\n"),routeId,caption);
|
||||
}
|
||||
|
||||
Print * CommandDistributor::getVirtualLCDSerial(byte screen, byte row) {
|
||||
Print * stream=virtualLCDSerial;
|
||||
#ifdef CD_HANDLE_RING
|
||||
rememberVLCDClient=RingStream::NO_CLIENT;
|
||||
if (!stream && virtualLCDClient!=RingStream::NO_CLIENT) {
|
||||
// If we are broadcasting from a wifi/eth process we need to complete its output
|
||||
// before merging broadcasts in the ring, then reinstate it in case
|
||||
// the process continues to output to its client.
|
||||
if ((rememberVLCDClient = ring->peekTargetMark()) != RingStream::NO_CLIENT) {
|
||||
ring->commit();
|
||||
}
|
||||
ring->mark(virtualLCDClient);
|
||||
stream=ring;
|
||||
}
|
||||
#endif
|
||||
if (stream) StringFormatter::send(stream,F("<@ %d %d \""), screen,row);
|
||||
return stream;
|
||||
}
|
||||
|
||||
void CommandDistributor::commitVirtualLCDSerial() {
|
||||
#ifdef CD_HANDLE_RING
|
||||
if (virtualLCDClient!=RingStream::NO_CLIENT) {
|
||||
StringFormatter::send(ring,F("\">\n"));
|
||||
ring->commit();
|
||||
if (rememberVLCDClient!=RingStream::NO_CLIENT) ring->mark(rememberVLCDClient);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
StringFormatter::send(virtualLCDSerial,F("\">\n"));
|
||||
}
|
||||
|
||||
void CommandDistributor::setVirtualLCDSerial(Print * stream) {
|
||||
#ifdef CD_HANDLE_RING
|
||||
virtualLCDClient=RingStream::NO_CLIENT;
|
||||
if (stream && stream->availableForWrite()==RingStream::THIS_IS_A_RINGSTREAM) {
|
||||
virtualLCDClient=((RingStream *) stream)->peekTargetMark();
|
||||
virtualLCDSerial=nullptr;
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
virtualLCDSerial=stream;
|
||||
}
|
||||
|
||||
Print* CommandDistributor::virtualLCDSerial=&USB_SERIAL;
|
||||
byte CommandDistributor::virtualLCDClient=0xFF;
|
||||
byte CommandDistributor::rememberVLCDClient=0;
|
||||
|
||||
|
@@ -55,20 +55,10 @@ public :
|
||||
static int16_t retClockTime();
|
||||
static void broadcastPower();
|
||||
static void broadcastRaw(clientType type,char * msg);
|
||||
static void broadcastTrackState(const FSH* format,byte trackLetter, const FSH* modename, int16_t dcAddr);
|
||||
static void broadcastTrackState(const FSH* format,byte trackLetter, int16_t dcAddr);
|
||||
template<typename... Targs> static void broadcastReply(clientType type, Targs... msg);
|
||||
static void forget(byte clientId);
|
||||
static void broadcastRouteState(uint16_t routeId,byte state);
|
||||
static void broadcastRouteCaption(uint16_t routeId,const FSH * caption);
|
||||
|
||||
// Handling code for virtual LCD receiver.
|
||||
static Print * getVirtualLCDSerial(byte screen, byte row);
|
||||
static void commitVirtualLCDSerial();
|
||||
static void setVirtualLCDSerial(Print * stream);
|
||||
private:
|
||||
static Print * virtualLCDSerial;
|
||||
static byte virtualLCDClient;
|
||||
static byte rememberVLCDClient;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
@@ -76,12 +76,6 @@ void setup()
|
||||
|
||||
DIAG(F("License GPLv3 fsf.org (c) dcc-ex.com"));
|
||||
|
||||
// If user has defined a startup delay, delay here before starting IO
|
||||
#if defined(STARTUP_DELAY)
|
||||
DIAG(F("Delaying startup for %dms"), STARTUP_DELAY);
|
||||
delay(STARTUP_DELAY);
|
||||
#endif
|
||||
|
||||
// Initialise HAL layer before reading EEprom or setting up MotorDrivers
|
||||
IODevice::begin();
|
||||
|
||||
@@ -93,7 +87,7 @@ void setup()
|
||||
|
||||
DISPLAY_START (
|
||||
// This block is still executed for DIAGS if display not in use
|
||||
LCD(0,F("DCC-EX v" VERSION));
|
||||
LCD(0,F("DCC-EX v%S"),F(VERSION));
|
||||
LCD(1,F("Lic GPLv3"));
|
||||
);
|
||||
|
||||
@@ -102,7 +96,11 @@ void setup()
|
||||
// Start Ethernet if it exists
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
#if WIFI_ON
|
||||
#ifndef WIFI_NINA
|
||||
WifiInterface::setup(WIFI_SERIAL_LINK_SPEED, F(WIFI_SSID), F(WIFI_PASSWORD), F(WIFI_HOSTNAME), IP_PORT, WIFI_CHANNEL, WIFI_FORCE_AP);
|
||||
#else
|
||||
WifiNINA::setup(WIFI_SSID, WIFI_PASSWORD, WIFI_HOSTNAME, IP_PORT, WIFI_CHANNEL, WIFI_FORCE_AP);
|
||||
#endif // WIFI_NINA
|
||||
#endif // WIFI_ON
|
||||
#else
|
||||
// ESP32 needs wifi on always
|
||||
@@ -150,7 +148,11 @@ void loop()
|
||||
// Responsibility 3: Optionally handle any incoming WiFi traffic
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
#if WIFI_ON
|
||||
#ifndef WIFI_NINA
|
||||
WifiInterface::loop();
|
||||
#else
|
||||
WifiNINA::loop();
|
||||
#endif //WIFI_NINA
|
||||
#endif //WIFI_ON
|
||||
#else //ARDUINO_ARCH_ESP32
|
||||
#ifndef WIFI_TASK_ON_CORE0
|
||||
|
31
DCC.cpp
31
DCC.cpp
@@ -122,7 +122,7 @@ void DCC::setThrottle2( uint16_t cab, byte speedCode) {
|
||||
DCCWaveform::mainTrack.schedulePacket(b, nB, 0);
|
||||
}
|
||||
|
||||
void DCC::setFunctionInternal(int cab, byte byte1, byte byte2, byte count) {
|
||||
void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) {
|
||||
// DIAG(F("setFunctionInternal %d %x %x"),cab,byte1,byte2);
|
||||
byte b[4];
|
||||
byte nB = 0;
|
||||
@@ -133,7 +133,7 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2, byte count) {
|
||||
if (byte1!=0) b[nB++] = byte1;
|
||||
b[nB++] = byte2;
|
||||
|
||||
DCCWaveform::mainTrack.schedulePacket(b, nB, count);
|
||||
DCCWaveform::mainTrack.schedulePacket(b, nB, 0);
|
||||
}
|
||||
|
||||
// returns speed steps 0 to 127 (1 == emergency stop)
|
||||
@@ -595,7 +595,7 @@ void DCC::loop() {
|
||||
|
||||
void DCC::issueReminders() {
|
||||
// if the main track transmitter still has a pending packet, skip this time around.
|
||||
if (!DCCWaveform::mainTrack.isReminderWindowOpen()) return;
|
||||
if ( DCCWaveform::mainTrack.getPacketPending()) return;
|
||||
// Move to next loco slot. If occupied, send a reminder.
|
||||
int reg = lastLocoReminder+1;
|
||||
if (reg > highestUsedReg) reg = 0; // Go to start of table
|
||||
@@ -619,39 +619,24 @@ bool DCC::issueReminder(int reg) {
|
||||
break;
|
||||
case 1: // remind function group 1 (F0-F4)
|
||||
if (flags & FN_GROUP_1)
|
||||
#ifndef DISABLE_FUNCTION_REMINDERS
|
||||
setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4),0); // 100D DDDD
|
||||
#else
|
||||
setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4),2);
|
||||
flags&= ~FN_GROUP_1; // dont send them again
|
||||
#endif
|
||||
setFunctionInternal(loco,0, 128 | ((functions>>1)& 0x0F) | ((functions & 0x01)<<4)); // 100D DDDD
|
||||
break;
|
||||
case 2: // remind function group 2 F5-F8
|
||||
if (flags & FN_GROUP_2)
|
||||
#ifndef DISABLE_FUNCTION_REMINDERS
|
||||
setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F),0); // 1011 DDDD
|
||||
#else
|
||||
setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F),2);
|
||||
flags&= ~FN_GROUP_2; // dont send them again
|
||||
#endif
|
||||
setFunctionInternal(loco,0, 176 | ((functions>>5)& 0x0F)); // 1011 DDDD
|
||||
break;
|
||||
case 3: // remind function group 3 F9-F12
|
||||
if (flags & FN_GROUP_3)
|
||||
#ifndef DISABLE_FUNCTION_REMINDERS
|
||||
setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F),0); // 1010 DDDD
|
||||
#else
|
||||
setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F),2);
|
||||
flags&= ~FN_GROUP_3; // dont send them again
|
||||
#endif
|
||||
setFunctionInternal(loco,0, 160 | ((functions>>9)& 0x0F)); // 1010 DDDD
|
||||
break;
|
||||
case 4: // remind function group 4 F13-F20
|
||||
if (flags & FN_GROUP_4)
|
||||
setFunctionInternal(loco,222, ((functions>>13)& 0xFF),2);
|
||||
setFunctionInternal(loco,222, ((functions>>13)& 0xFF));
|
||||
flags&= ~FN_GROUP_4; // dont send them again
|
||||
break;
|
||||
case 5: // remind function group 5 F21-F28
|
||||
if (flags & FN_GROUP_5)
|
||||
setFunctionInternal(loco,223, ((functions>>21)& 0xFF),2);
|
||||
setFunctionInternal(loco,223, ((functions>>21)& 0xFF));
|
||||
flags&= ~FN_GROUP_5; // dont send them again
|
||||
break;
|
||||
}
|
||||
|
2
DCC.h
2
DCC.h
@@ -109,7 +109,7 @@ private:
|
||||
static byte loopStatus;
|
||||
static void setThrottle2(uint16_t cab, uint8_t speedCode);
|
||||
static void updateLocoReminder(int loco, byte speedCode);
|
||||
static void setFunctionInternal(int cab, byte fByte, byte eByte, byte count);
|
||||
static void setFunctionInternal(int cab, byte fByte, byte eByte);
|
||||
static bool issueReminder(int reg);
|
||||
static int lastLocoReminder;
|
||||
static int highestUsedReg;
|
||||
|
9
DCCEX.h
9
DCCEX.h
@@ -1,4 +1,5 @@
|
||||
/*
|
||||
* © 2023 Paul M. Antoine
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2021 Harald Barth
|
||||
* © 2020-2021 Chris Harlow
|
||||
@@ -33,8 +34,13 @@
|
||||
#include "SerialManager.h"
|
||||
#include "version.h"
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
#include "WifiInterface.h"
|
||||
#ifdef WIFI_NINA
|
||||
#include "Wifi_NINA.h"
|
||||
#else
|
||||
#include "WifiInterface.h"
|
||||
#endif // WIFI_NINA
|
||||
#else
|
||||
#undef WIFI_NINA
|
||||
#include "WifiESP32.h"
|
||||
#endif
|
||||
#if ETHERNET_ON == true
|
||||
@@ -49,7 +55,6 @@
|
||||
#include "CommandDistributor.h"
|
||||
#include "TrackManager.h"
|
||||
#include "DCCTimer.h"
|
||||
#include "KeywordHasher.h"
|
||||
#include "EXRAIL.h"
|
||||
|
||||
#endif
|
||||
|
366
DCCEXParser.cpp
366
DCCEXParser.cpp
@@ -115,8 +115,6 @@ Once a new OPCODE is decided upon, update this list.
|
||||
#include "DCCTimer.h"
|
||||
#include "EXRAIL2.h"
|
||||
#include "Turntables.h"
|
||||
#include "version.h"
|
||||
#include "KeywordHasher.h"
|
||||
|
||||
// This macro can't be created easily as a portable function because the
|
||||
// flashlist requires a far pointer for high flash access.
|
||||
@@ -127,6 +125,56 @@ Once a new OPCODE is decided upon, update this list.
|
||||
StringFormatter::send(stream,F(" %d"),value); \
|
||||
}
|
||||
|
||||
|
||||
// These keywords are used in the <1> command. The number is what you get if you use the keyword as a parameter.
|
||||
// To discover new keyword numbers , use the <$ YOURKEYWORD> command
|
||||
const int16_t HASH_KEYWORD_MAIN = 11339;
|
||||
const int16_t HASH_KEYWORD_CABS = -11981;
|
||||
const int16_t HASH_KEYWORD_RAM = 25982;
|
||||
const int16_t HASH_KEYWORD_CMD = 9962;
|
||||
const int16_t HASH_KEYWORD_ACK = 3113;
|
||||
const int16_t HASH_KEYWORD_ON = 2657;
|
||||
const int16_t HASH_KEYWORD_DCC = 6436;
|
||||
const int16_t HASH_KEYWORD_SLOW = -17209;
|
||||
#ifndef DISABLE_PROG
|
||||
const int16_t HASH_KEYWORD_JOIN = -30750;
|
||||
const int16_t HASH_KEYWORD_PROG = -29718;
|
||||
const int16_t HASH_KEYWORD_PROGBOOST = -6353;
|
||||
#endif
|
||||
#ifndef DISABLE_EEPROM
|
||||
const int16_t HASH_KEYWORD_EEPROM = -7168;
|
||||
#endif
|
||||
const int16_t HASH_KEYWORD_LIMIT = 27413;
|
||||
const int16_t HASH_KEYWORD_MAX = 16244;
|
||||
const int16_t HASH_KEYWORD_MIN = 15978;
|
||||
const int16_t HASH_KEYWORD_RESET = 26133;
|
||||
const int16_t HASH_KEYWORD_RETRY = 25704;
|
||||
const int16_t HASH_KEYWORD_SPEED28 = -17064;
|
||||
const int16_t HASH_KEYWORD_SPEED128 = 25816;
|
||||
const int16_t HASH_KEYWORD_SERVO=27709;
|
||||
const int16_t HASH_KEYWORD_TT=2688;
|
||||
const int16_t HASH_KEYWORD_VPIN=-415;
|
||||
const int16_t HASH_KEYWORD_A='A';
|
||||
const int16_t HASH_KEYWORD_C='C';
|
||||
const int16_t HASH_KEYWORD_G='G';
|
||||
const int16_t HASH_KEYWORD_H='H';
|
||||
const int16_t HASH_KEYWORD_I='I';
|
||||
const int16_t HASH_KEYWORD_O='O';
|
||||
const int16_t HASH_KEYWORD_P='P';
|
||||
const int16_t HASH_KEYWORD_R='R';
|
||||
const int16_t HASH_KEYWORD_T='T';
|
||||
const int16_t HASH_KEYWORD_X='X';
|
||||
const int16_t HASH_KEYWORD_LCN = 15137;
|
||||
const int16_t HASH_KEYWORD_HAL = 10853;
|
||||
const int16_t HASH_KEYWORD_SHOW = -21309;
|
||||
const int16_t HASH_KEYWORD_ANIN = -10424;
|
||||
const int16_t HASH_KEYWORD_ANOUT = -26399;
|
||||
const int16_t HASH_KEYWORD_WIFI = -5583;
|
||||
const int16_t HASH_KEYWORD_ETHERNET = -30767;
|
||||
const int16_t HASH_KEYWORD_WIT = 31594;
|
||||
const int16_t HASH_KEYWORD_EXTT = 8573;
|
||||
const int16_t HASH_KEYWORD_ADD = 3201;
|
||||
|
||||
int16_t DCCEXParser::stashP[MAX_COMMAND_PARAMS];
|
||||
bool DCCEXParser::stashBusy;
|
||||
Print *DCCEXParser::stashStream = NULL;
|
||||
@@ -162,10 +210,8 @@ int16_t DCCEXParser::splitValues(int16_t result[MAX_COMMAND_PARAMS], const byte
|
||||
case 1: // skipping spaces before a param
|
||||
if (hot == ' ')
|
||||
break;
|
||||
if (hot == '\0')
|
||||
return -1;
|
||||
if (hot == '>')
|
||||
return parameterCount;
|
||||
if (hot == '\0' || hot == '>')
|
||||
return parameterCount;
|
||||
state = 2;
|
||||
continue;
|
||||
|
||||
@@ -258,19 +304,14 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
#ifndef DISABLE_EEPROM
|
||||
(void)EEPROM; // tell compiler not to warn this is unused
|
||||
#endif
|
||||
byte params = 0;
|
||||
if (Diag::CMD)
|
||||
DIAG(F("PARSING:%s"), com);
|
||||
int16_t p[MAX_COMMAND_PARAMS];
|
||||
while (com[0] == '<' || com[0] == ' ')
|
||||
com++; // strip off any number of < or spaces
|
||||
byte opcode = com[0];
|
||||
int16_t splitnum = splitValues(p, com, opcode=='M' || opcode=='P');
|
||||
if (splitnum < 0 || splitnum >= MAX_COMMAND_PARAMS) // if arguments are broken, leave but via printing <X>
|
||||
goto out;
|
||||
// Because of check above we are now inside byte size
|
||||
params = splitnum;
|
||||
|
||||
byte params = splitValues(p, com, opcode=='M' || opcode=='P');
|
||||
|
||||
if (filterCallback)
|
||||
filterCallback(stream, opcode, params, p);
|
||||
if (filterRMFTCallback && opcode!='\0')
|
||||
@@ -512,64 +553,131 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
|
||||
case '1': // POWERON <1 [MAIN|PROG|JOIN]>
|
||||
{
|
||||
if (params > 1) break;
|
||||
if (params==0) { // All
|
||||
TrackManager::setTrackPower(TRACK_MODE_ALL, POWERMODE::ON);
|
||||
}
|
||||
if (params==1) {
|
||||
if (p[0]=="MAIN"_hk) { // <1 MAIN>
|
||||
TrackManager::setTrackPower(TRACK_MODE_MAIN, POWERMODE::ON);
|
||||
bool main=false;
|
||||
bool prog=false;
|
||||
bool join=false;
|
||||
bool singletrack=false;
|
||||
//byte t=0;
|
||||
if (params > 1) break;
|
||||
if (params==0) { // All
|
||||
main=true;
|
||||
prog=true;
|
||||
}
|
||||
if (params==1) {
|
||||
if (p[0]==HASH_KEYWORD_MAIN) { // <1 MAIN>
|
||||
main=true;
|
||||
}
|
||||
#ifndef DISABLE_PROG
|
||||
else if (p[0] == "JOIN"_hk) { // <1 JOIN>
|
||||
TrackManager::setJoin(true);
|
||||
TrackManager::setTrackPower(TRACK_MODE_MAIN|TRACK_MODE_PROG, POWERMODE::ON);
|
||||
else if (p[0] == HASH_KEYWORD_JOIN) { // <1 JOIN>
|
||||
main=true;
|
||||
prog=true;
|
||||
join=true;
|
||||
}
|
||||
else if (p[0]=="PROG"_hk) { // <1 PROG>
|
||||
TrackManager::setJoin(false);
|
||||
TrackManager::setTrackPower(TRACK_MODE_PROG, POWERMODE::ON);
|
||||
else if (p[0]==HASH_KEYWORD_PROG) { // <1 PROG>
|
||||
prog=true;
|
||||
}
|
||||
#endif
|
||||
else if (p[0] >= "A"_hk && p[0] <= "H"_hk) { // <1 A-H>
|
||||
byte t = (p[0] - 'A');
|
||||
TrackManager::setTrackPower(POWERMODE::ON, t);
|
||||
//StringFormatter::send(stream, F("<p1 %c>\n"), t+'A');
|
||||
//else if (p[0] >= 'A' && p[0] <= 'H') { // <1 A-H>
|
||||
else if (p[0] >= HASH_KEYWORD_A && p[0] <= HASH_KEYWORD_H) { // <1 A-H>
|
||||
byte t = (p[0] - 'A');
|
||||
//DIAG(F("Processing track - %d "), t);
|
||||
if (TrackManager::isProg(t)) {
|
||||
main = false;
|
||||
prog = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
main=true;
|
||||
prog=false;
|
||||
}
|
||||
singletrack=true;
|
||||
if (main) TrackManager::setTrackPower(false, false, POWERMODE::ON, t);
|
||||
if (prog) TrackManager::setTrackPower(true, false, POWERMODE::ON, t);
|
||||
|
||||
StringFormatter::send(stream, F("<1 %c>\n"), t+'A');
|
||||
//CommandDistributor::broadcastPower();
|
||||
//TrackManager::streamTrackState(NULL,t);
|
||||
return;
|
||||
}
|
||||
|
||||
else break; // will reply <X>
|
||||
}
|
||||
//TrackManager::streamTrackState(NULL,t);
|
||||
}
|
||||
|
||||
if (!singletrack) {
|
||||
TrackManager::setJoin(join);
|
||||
if (join) TrackManager::setJoinPower(POWERMODE::ON);
|
||||
else {
|
||||
if (main) TrackManager::setMainPower(POWERMODE::ON);
|
||||
if (prog) TrackManager::setProgPower(POWERMODE::ON);
|
||||
}
|
||||
CommandDistributor::broadcastPower();
|
||||
|
||||
return;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
case '0': // POWEROFF <0 [MAIN | PROG] >
|
||||
{
|
||||
if (params > 1) break;
|
||||
if (params==0) { // All
|
||||
TrackManager::setJoin(false);
|
||||
TrackManager::setTrackPower(TRACK_MODE_ALL, POWERMODE::OFF);
|
||||
}
|
||||
if (params==1) {
|
||||
if (p[0]=="MAIN"_hk) { // <0 MAIN>
|
||||
TrackManager::setJoin(false);
|
||||
TrackManager::setTrackPower(TRACK_MODE_MAIN, POWERMODE::OFF);
|
||||
}
|
||||
bool main=false;
|
||||
bool prog=false;
|
||||
bool singletrack=false;
|
||||
//byte t=0;
|
||||
if (params > 1) break;
|
||||
if (params==0) { // All
|
||||
main=true;
|
||||
prog=true;
|
||||
}
|
||||
if (params==1) {
|
||||
if (p[0]==HASH_KEYWORD_MAIN) { // <0 MAIN>
|
||||
main=true;
|
||||
}
|
||||
#ifndef DISABLE_PROG
|
||||
else if (p[0]=="PROG"_hk) { // <0 PROG>
|
||||
TrackManager::progTrackBoosted=false; // Prog track boost mode will not outlive prog track off
|
||||
TrackManager::setTrackPower(TRACK_MODE_PROG, POWERMODE::OFF);
|
||||
else if (p[0]==HASH_KEYWORD_PROG) { // <0 PROG>
|
||||
prog=true;
|
||||
}
|
||||
#endif
|
||||
else if (p[0] >= "A"_hk && p[0] <= "H"_hk) { // <1 A-H>
|
||||
byte t = (p[0] - 'A');
|
||||
TrackManager::setJoin(false);
|
||||
TrackManager::setTrackPower(POWERMODE::OFF, t);
|
||||
//StringFormatter::send(stream, F("<p0 %c>\n"), t+'A');
|
||||
}
|
||||
//else if (p[0] >= 'A' && p[0] <= 'H') { // <1 A-H>
|
||||
else if (p[0] >= HASH_KEYWORD_A && p[0] <= HASH_KEYWORD_H) { // <1 A-H>
|
||||
byte t = (p[0] - 'A');
|
||||
//DIAG(F("Processing track - %d "), t);
|
||||
if (TrackManager::isProg(t)) {
|
||||
main = false;
|
||||
prog = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
main=true;
|
||||
prog=false;
|
||||
}
|
||||
singletrack=true;
|
||||
TrackManager::setJoin(false);
|
||||
if (main) TrackManager::setTrackPower(false, false, POWERMODE::OFF, t);
|
||||
if (prog) {
|
||||
TrackManager::progTrackBoosted=false; // Prog track boost mode will not outlive prog track off
|
||||
TrackManager::setTrackPower(true, false, POWERMODE::OFF, t);
|
||||
}
|
||||
StringFormatter::send(stream, F("<0 %c>\n"), t+'A');
|
||||
//CommandDistributor::broadcastPower();
|
||||
//TrackManager::streamTrackState(NULL, t);
|
||||
return;
|
||||
}
|
||||
|
||||
else break; // will reply <X>
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (!singletrack) {
|
||||
TrackManager::setJoin(false);
|
||||
|
||||
if (main) TrackManager::setMainPower(POWERMODE::OFF);
|
||||
if (prog) {
|
||||
TrackManager::progTrackBoosted=false; // Prog track boost mode will not outlive prog track off
|
||||
TrackManager::setProgPower(POWERMODE::OFF);
|
||||
}
|
||||
CommandDistributor::broadcastPower();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
case '!': // ESTOP ALL <!>
|
||||
DCC::setThrottle(0,1,1); // this broadcasts speed 1(estop) and sets all reminders to speed 1.
|
||||
@@ -616,8 +724,8 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
return;
|
||||
break;
|
||||
#endif
|
||||
case '=': // TRACK MANAGER CONTROL <= [params]>
|
||||
if (TrackManager::parseEqualSign(stream, params, p))
|
||||
case '=': // TACK MANAGER CONTROL <= [params]>
|
||||
if (TrackManager::parseJ(stream, params, p))
|
||||
return;
|
||||
break;
|
||||
|
||||
@@ -654,7 +762,7 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
//if ((params<1) | (params>2)) break; // <J>
|
||||
int16_t id=(params==2)?p[1]:0;
|
||||
switch(p[0]) {
|
||||
case "C"_hk: // <JC mmmm nn> sets time and speed
|
||||
case HASH_KEYWORD_C: // <JC mmmm nn> sets time and speed
|
||||
if (params==1) { // <JC> returns latest time
|
||||
int16_t x = CommandDistributor::retClockTime();
|
||||
StringFormatter::send(stream, F("<jC %d>\n"), x);
|
||||
@@ -663,28 +771,38 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
CommandDistributor::setClockTime(p[1], p[2], 1);
|
||||
return;
|
||||
|
||||
case "G"_hk: // <JG> current gauge limits
|
||||
case HASH_KEYWORD_G: // <JG> current gauge limits
|
||||
if (params>1) break;
|
||||
TrackManager::reportGauges(stream); // <g limit...limit>
|
||||
return;
|
||||
|
||||
case "I"_hk: // <JI> current values
|
||||
case HASH_KEYWORD_I: // <JI> current values
|
||||
if (params>1) break;
|
||||
TrackManager::reportCurrent(stream); // <g limit...limit>
|
||||
return;
|
||||
|
||||
case "A"_hk: // <JA> intercepted by EXRAIL// <JA> returns automations/routes
|
||||
if (params!=1) break; // <JA>
|
||||
StringFormatter::send(stream, F("<jA>\n"));
|
||||
return;
|
||||
|
||||
case "M"_hk: // <JM> intercepted by EXRAIL
|
||||
if (params>1) break; // invalid cant do
|
||||
// <JM> requests stash size so say none.
|
||||
StringFormatter::send(stream,F("<jM 0>\n"));
|
||||
return;
|
||||
|
||||
case "R"_hk: // <JR> returns rosters
|
||||
case HASH_KEYWORD_A: // <JA> returns automations/routes
|
||||
StringFormatter::send(stream, F("<jA"));
|
||||
if (params==1) {// <JA>
|
||||
#ifdef EXRAIL_ACTIVE
|
||||
SENDFLASHLIST(stream,RMFT2::routeIdList)
|
||||
SENDFLASHLIST(stream,RMFT2::automationIdList)
|
||||
#endif
|
||||
}
|
||||
else { // <JA id>
|
||||
StringFormatter::send(stream,F(" %d %c \"%S\""),
|
||||
id,
|
||||
#ifdef EXRAIL_ACTIVE
|
||||
RMFT2::getRouteType(id), // A/R
|
||||
RMFT2::getRouteDescription(id)
|
||||
#else
|
||||
'X',F("")
|
||||
#endif
|
||||
);
|
||||
}
|
||||
StringFormatter::send(stream, F(">\n"));
|
||||
return;
|
||||
case HASH_KEYWORD_R: // <JR> returns rosters
|
||||
StringFormatter::send(stream, F("<jR"));
|
||||
#ifdef EXRAIL_ACTIVE
|
||||
if (params==1) {
|
||||
@@ -703,7 +821,7 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
#endif
|
||||
StringFormatter::send(stream, F(">\n"));
|
||||
return;
|
||||
case "T"_hk: // <JT> returns turnout list
|
||||
case HASH_KEYWORD_T: // <JT> returns turnout list
|
||||
StringFormatter::send(stream, F("<jT"));
|
||||
if (params==1) { // <JT>
|
||||
for ( Turnout * t=Turnout::first(); t; t=t->next()) {
|
||||
@@ -730,7 +848,7 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
return;
|
||||
// No turntables without HAL support
|
||||
#ifndef IO_NO_HAL
|
||||
case "O"_hk: // <JO returns turntable list
|
||||
case HASH_KEYWORD_O: // <JO returns turntable list
|
||||
StringFormatter::send(stream, F("<jO"));
|
||||
if (params==1) { // <JO>
|
||||
for (Turntable * tto=Turntable::first(); tto; tto=tto->next()) {
|
||||
@@ -755,7 +873,7 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
}
|
||||
}
|
||||
return;
|
||||
case "P"_hk: // <JP id> returns turntable position list for the turntable id
|
||||
case HASH_KEYWORD_P: // <JP id> returns turntable position list for the turntable id
|
||||
if (params==2) { // <JP id>
|
||||
Turntable *tto=Turntable::get(id);
|
||||
if (!tto || tto->isHidden()) {
|
||||
@@ -795,27 +913,15 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
|
||||
case 'L': // LCC interface implemented in EXRAIL parser
|
||||
break; // Will <X> if not intercepted by EXRAIL
|
||||
|
||||
#ifndef DISABLE_VDPY
|
||||
case '@': // JMRI saying "give me virtual LCD msgs"
|
||||
CommandDistributor::setVirtualLCDSerial(stream);
|
||||
StringFormatter::send(stream,
|
||||
F("<@ 0 0 \"DCC-EX v" VERSION "\">\n"
|
||||
"<@ 0 1 \"Lic GPLv3\">\n"));
|
||||
return;
|
||||
#endif
|
||||
default: //anything else will diagnose and drop out to <X>
|
||||
if (opcode >= ' ' && opcode <= '~') {
|
||||
DIAG(F("Opcode=%c params=%d"), opcode, params);
|
||||
for (int i = 0; i < params; i++)
|
||||
DIAG(F("p[%d]=%d (0x%x)"), i, p[i], p[i]);
|
||||
} else {
|
||||
DIAG(F("Unprintable %x"), opcode);
|
||||
}
|
||||
break;
|
||||
break;
|
||||
|
||||
} // end of opcode switch
|
||||
|
||||
out:// Any fallout here sends an <X>
|
||||
// Any fallout here sends an <X>
|
||||
StringFormatter::send(stream, F("<X>\n"));
|
||||
}
|
||||
|
||||
@@ -922,14 +1028,14 @@ bool DCCEXParser::parseT(Print *stream, int16_t params, int16_t p[])
|
||||
switch (p[1]) {
|
||||
// Turnout messages use 1=throw, 0=close.
|
||||
case 0:
|
||||
case "C"_hk:
|
||||
case HASH_KEYWORD_C:
|
||||
state = true;
|
||||
break;
|
||||
case 1:
|
||||
case "T"_hk:
|
||||
case HASH_KEYWORD_T:
|
||||
state= false;
|
||||
break;
|
||||
case "X"_hk:
|
||||
case HASH_KEYWORD_X:
|
||||
{
|
||||
Turnout *tt = Turnout::get(p[0]);
|
||||
if (tt) {
|
||||
@@ -946,14 +1052,14 @@ bool DCCEXParser::parseT(Print *stream, int16_t params, int16_t p[])
|
||||
}
|
||||
|
||||
default: // Anything else is some kind of turnout create function.
|
||||
if (params == 6 && p[1] == "SERVO"_hk) { // <T id SERVO n n n n>
|
||||
if (params == 6 && p[1] == HASH_KEYWORD_SERVO) { // <T id SERVO n n n n>
|
||||
if (!ServoTurnout::create(p[0], (VPIN)p[2], (uint16_t)p[3], (uint16_t)p[4], (uint8_t)p[5]))
|
||||
return false;
|
||||
} else
|
||||
if (params == 3 && p[1] == "VPIN"_hk) { // <T id VPIN n>
|
||||
if (params == 3 && p[1] == HASH_KEYWORD_VPIN) { // <T id VPIN n>
|
||||
if (!VpinTurnout::create(p[0], p[2])) return false;
|
||||
} else
|
||||
if (params >= 3 && p[1] == "DCC"_hk) {
|
||||
if (params >= 3 && p[1] == HASH_KEYWORD_DCC) {
|
||||
// <T id DCC addr subadd> 0<=addr<=511, 0<=subadd<=3 (like <a> command).<T>
|
||||
if (params==4 && p[2]>=0 && p[2]<512 && p[3]>=0 && p[3]<4) { // <T id DCC n m>
|
||||
if (!DCCTurnout::create(p[0], p[2], p[3])) return false;
|
||||
@@ -1013,48 +1119,46 @@ bool DCCEXParser::parseS(Print *stream, int16_t params, int16_t p[])
|
||||
}
|
||||
|
||||
bool DCCEXParser::parseC(Print *stream, int16_t params, int16_t p[]) {
|
||||
(void)stream; // arg not used, maybe later?
|
||||
if (params == 0)
|
||||
return false;
|
||||
bool onOff = (params > 0) && (p[1] == 1 || p[1] == HASH_KEYWORD_ON); // dont care if other stuff or missing... just means off
|
||||
switch (p[0])
|
||||
{
|
||||
#ifndef DISABLE_PROG
|
||||
case "PROGBOOST"_hk:
|
||||
case HASH_KEYWORD_PROGBOOST:
|
||||
TrackManager::progTrackBoosted=true;
|
||||
return true;
|
||||
#endif
|
||||
case "RESET"_hk:
|
||||
case HASH_KEYWORD_RESET:
|
||||
DCCTimer::reset();
|
||||
break; // and <X> if we didnt restart
|
||||
case "SPEED28"_hk:
|
||||
case HASH_KEYWORD_SPEED28:
|
||||
DCC::setGlobalSpeedsteps(28);
|
||||
DIAG(F("28 Speedsteps"));
|
||||
return true;
|
||||
|
||||
case "SPEED128"_hk:
|
||||
case HASH_KEYWORD_SPEED128:
|
||||
DCC::setGlobalSpeedsteps(128);
|
||||
DIAG(F("128 Speedsteps"));
|
||||
return true;
|
||||
|
||||
#ifndef DISABLE_PROG
|
||||
case "ACK"_hk: // <D ACK ON/OFF> <D ACK [LIMIT|MIN|MAX|RETRY] Value>
|
||||
case HASH_KEYWORD_ACK: // <D ACK ON/OFF> <D ACK [LIMIT|MIN|MAX|RETRY] Value>
|
||||
if (params >= 3) {
|
||||
if (p[1] == "LIMIT"_hk) {
|
||||
if (p[1] == HASH_KEYWORD_LIMIT) {
|
||||
DCCACK::setAckLimit(p[2]);
|
||||
LCD(1, F("Ack Limit=%dmA"), p[2]); // <D ACK LIMIT 42>
|
||||
} else if (p[1] == "MIN"_hk) {
|
||||
} else if (p[1] == HASH_KEYWORD_MIN) {
|
||||
DCCACK::setMinAckPulseDuration(p[2]);
|
||||
LCD(0, F("Ack Min=%uus"), p[2]); // <D ACK MIN 1500>
|
||||
} else if (p[1] == "MAX"_hk) {
|
||||
} else if (p[1] == HASH_KEYWORD_MAX) {
|
||||
DCCACK::setMaxAckPulseDuration(p[2]);
|
||||
LCD(0, F("Ack Max=%uus"), p[2]); // <D ACK MAX 9000>
|
||||
} else if (p[1] == "RETRY"_hk) {
|
||||
} else if (p[1] == HASH_KEYWORD_RETRY) {
|
||||
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>
|
||||
}
|
||||
} else {
|
||||
bool onOff = (params > 0) && (p[1] == 1 || p[1] == "ON"_hk); // dont care if other stuff or missing... just means off
|
||||
|
||||
DIAG(F("Ack diag %S"), onOff ? F("on") : F("off"));
|
||||
Diag::ACK = onOff;
|
||||
}
|
||||
@@ -1071,66 +1175,66 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
|
||||
{
|
||||
if (params == 0)
|
||||
return false;
|
||||
bool onOff = (params > 0) && (p[1] == 1 || p[1] == "ON"_hk); // dont care if other stuff or missing... just means off
|
||||
bool onOff = (params > 0) && (p[1] == 1 || p[1] == HASH_KEYWORD_ON); // dont care if other stuff or missing... just means off
|
||||
switch (p[0])
|
||||
{
|
||||
case "CABS"_hk: // <D CABS>
|
||||
case HASH_KEYWORD_CABS: // <D CABS>
|
||||
DCC::displayCabList(stream);
|
||||
return true;
|
||||
|
||||
case "RAM"_hk: // <D RAM>
|
||||
case HASH_KEYWORD_RAM: // <D RAM>
|
||||
DIAG(F("Free memory=%d"), DCCTimer::getMinimumFreeMemory());
|
||||
return true;
|
||||
|
||||
case "CMD"_hk: // <D CMD ON/OFF>
|
||||
case HASH_KEYWORD_CMD: // <D CMD ON/OFF>
|
||||
Diag::CMD = onOff;
|
||||
return true;
|
||||
|
||||
#ifdef HAS_ENOUGH_MEMORY
|
||||
case "WIFI"_hk: // <D WIFI ON/OFF>
|
||||
case HASH_KEYWORD_WIFI: // <D WIFI ON/OFF>
|
||||
Diag::WIFI = onOff;
|
||||
return true;
|
||||
|
||||
case "ETHERNET"_hk: // <D ETHERNET ON/OFF>
|
||||
case HASH_KEYWORD_ETHERNET: // <D ETHERNET ON/OFF>
|
||||
Diag::ETHERNET = onOff;
|
||||
return true;
|
||||
|
||||
case "WIT"_hk: // <D WIT ON/OFF>
|
||||
case HASH_KEYWORD_WIT: // <D WIT ON/OFF>
|
||||
Diag::WITHROTTLE = onOff;
|
||||
return true;
|
||||
|
||||
case "LCN"_hk: // <D LCN ON/OFF>
|
||||
case HASH_KEYWORD_LCN: // <D LCN ON/OFF>
|
||||
Diag::LCN = onOff;
|
||||
return true;
|
||||
#endif
|
||||
#ifndef DISABLE_EEPROM
|
||||
case "EEPROM"_hk: // <D EEPROM NumEntries>
|
||||
case HASH_KEYWORD_EEPROM: // <D EEPROM NumEntries>
|
||||
if (params >= 2)
|
||||
EEStore::dump(p[1]);
|
||||
return true;
|
||||
#endif
|
||||
case "SERVO"_hk: // <D SERVO vpin position [profile]>
|
||||
case HASH_KEYWORD_SERVO: // <D SERVO vpin position [profile]>
|
||||
|
||||
case "ANOUT"_hk: // <D ANOUT vpin position [profile]>
|
||||
case HASH_KEYWORD_ANOUT: // <D ANOUT vpin position [profile]>
|
||||
IODevice::writeAnalogue(p[1], p[2], params>3 ? p[3] : 0);
|
||||
return true;
|
||||
break;
|
||||
|
||||
case "ANIN"_hk: // <D ANIN vpin> Display analogue input value
|
||||
case HASH_KEYWORD_ANIN: // <D ANIN vpin> Display analogue input value
|
||||
DIAG(F("VPIN=%u value=%d"), p[1], IODevice::readAnalogue(p[1]));
|
||||
return true;
|
||||
break;
|
||||
|
||||
#if !defined(IO_NO_HAL)
|
||||
case "HAL"_hk:
|
||||
if (p[1] == "SHOW"_hk)
|
||||
case HASH_KEYWORD_HAL:
|
||||
if (p[1] == HASH_KEYWORD_SHOW)
|
||||
IODevice::DumpAll();
|
||||
else if (p[1] == "RESET"_hk)
|
||||
else if (p[1] == HASH_KEYWORD_RESET)
|
||||
IODevice::reset();
|
||||
return true;
|
||||
break;
|
||||
#endif
|
||||
|
||||
case "TT"_hk: // <D TT vpin steps activity>
|
||||
case HASH_KEYWORD_TT: // <D TT vpin steps activity>
|
||||
IODevice::writeAnalogue(p[1], p[2], params>3 ? p[3] : 0);
|
||||
return true;
|
||||
break;
|
||||
|
||||
default: // invalid/unknown
|
||||
return parseC(stream, params, p);
|
||||
@@ -1182,7 +1286,7 @@ bool DCCEXParser::parseI(Print *stream, int16_t params, int16_t p[])
|
||||
case 3: // <I id position activity> | <I id DCC home> - rotate to position for EX-Turntable or create DCC turntable
|
||||
{
|
||||
Turntable *tto = Turntable::get(p[0]);
|
||||
if (p[1] == "DCC"_hk) {
|
||||
if (p[1] == HASH_KEYWORD_DCC) {
|
||||
if (tto || p[2] < 0 || p[2] > 3600) return false;
|
||||
if (!DCCTurntable::create(p[0])) return false;
|
||||
Turntable *tto = Turntable::get(p[0]);
|
||||
@@ -1199,7 +1303,7 @@ bool DCCEXParser::parseI(Print *stream, int16_t params, int16_t p[])
|
||||
case 4: // <I id EXTT vpin home> create an EXTT turntable
|
||||
{
|
||||
Turntable *tto = Turntable::get(p[0]);
|
||||
if (p[1] == "EXTT"_hk) {
|
||||
if (p[1] == HASH_KEYWORD_EXTT) {
|
||||
if (tto || p[3] < 0 || p[3] > 3600) return false;
|
||||
if (!EXTTTurntable::create(p[0], (VPIN)p[2])) return false;
|
||||
Turntable *tto = Turntable::get(p[0]);
|
||||
@@ -1214,7 +1318,7 @@ bool DCCEXParser::parseI(Print *stream, int16_t params, int16_t p[])
|
||||
case 5: // <I id ADD position value angle> add a position
|
||||
{
|
||||
Turntable *tto = Turntable::get(p[0]);
|
||||
if (p[1] == "ADD"_hk) {
|
||||
if (p[1] == HASH_KEYWORD_ADD) {
|
||||
// tto must exist, no more than 48 positions, angle 0 - 3600
|
||||
if (!tto || p[2] > 48 || p[4] < 0 || p[4] > 3600) return false;
|
||||
tto->addPosition(p[2], p[3], p[4]);
|
||||
|
50
DCCRMT.cpp
50
DCCRMT.cpp
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
* © 2021-2024, Harald Barth.
|
||||
* © 2021-2022, Harald Barth.
|
||||
*
|
||||
* This file is part of DCC-EX
|
||||
*
|
||||
@@ -25,18 +25,6 @@
|
||||
#include "DCCWaveform.h" // for MAX_PACKET_SIZE
|
||||
#include "soc/gpio_sig_map.h"
|
||||
|
||||
// check for right type of ESP32
|
||||
#include "soc/soc_caps.h"
|
||||
#ifndef SOC_RMT_MEM_WORDS_PER_CHANNEL
|
||||
#error This symobol should be defined
|
||||
#endif
|
||||
#if SOC_RMT_MEM_WORDS_PER_CHANNEL < 64
|
||||
#warning This is not an ESP32-WROOM but some other unsupported variant
|
||||
#warning You are outside of the DCC-EX supported hardware
|
||||
#endif
|
||||
|
||||
static const byte RMT_CHAN_PER_DCC_CHAN = 2;
|
||||
|
||||
// Number of bits resulting out of X bytes of DCC payload data
|
||||
// Each byte has one bit extra and at the end we have one EOF marker
|
||||
#define DATA_LEN(X) ((X)*9+1)
|
||||
@@ -87,30 +75,12 @@ void IRAM_ATTR interrupt(rmt_channel_t channel, void *t) {
|
||||
RMTChannel::RMTChannel(pinpair pins, bool isMain) {
|
||||
byte ch;
|
||||
byte plen;
|
||||
|
||||
// Below we check if the DCC packet actually fits into the RMT hardware
|
||||
// Currently MAX_PACKET_SIZE = 5 so with checksum there are
|
||||
// MAX_PACKET_SIZE+1 data packets. Each need DATA_LEN (9) bits.
|
||||
// To that we add the preamble length, the fencepost DCC end bit
|
||||
// and the RMT EOF marker.
|
||||
// SOC_RMT_MEM_WORDS_PER_CHANNEL is either 64 (original WROOM) or
|
||||
// 48 (all other ESP32 like the -C3 or -S2
|
||||
// The formula to get the possible MAX_PACKET_SIZE is
|
||||
//
|
||||
// ALLOCATED = RMT_CHAN_PER_DCC_CHAN * SOC_RMT_MEM_WORDS_PER_CHANNEL
|
||||
// MAX_PACKET_SIZE = floor((ALLOCATED - PREAMBLE_LEN - 2)/9 - 1)
|
||||
//
|
||||
|
||||
if (isMain) {
|
||||
ch = 0;
|
||||
plen = PREAMBLE_BITS_MAIN;
|
||||
static_assert (DATA_LEN(MAX_PACKET_SIZE+1) + PREAMBLE_BITS_MAIN + 2 <= RMT_CHAN_PER_DCC_CHAN * SOC_RMT_MEM_WORDS_PER_CHANNEL,
|
||||
"Number of DCC packet bits greater than ESP32 RMT memory available");
|
||||
} else {
|
||||
ch = RMT_CHAN_PER_DCC_CHAN; // number == offset
|
||||
ch = 2;
|
||||
plen = PREAMBLE_BITS_PROG;
|
||||
static_assert (DATA_LEN(MAX_PACKET_SIZE+1) + PREAMBLE_BITS_PROG + 2 <= RMT_CHAN_PER_DCC_CHAN * SOC_RMT_MEM_WORDS_PER_CHANNEL,
|
||||
"Number of DCC packet bits greater than ESP32 RMT memory available");
|
||||
}
|
||||
|
||||
// preamble
|
||||
@@ -145,7 +115,7 @@ RMTChannel::RMTChannel(pinpair pins, bool isMain) {
|
||||
// data: max packet size today is 5 + checksum
|
||||
maxDataLen = DATA_LEN(MAX_PACKET_SIZE+1); // plus checksum
|
||||
data = (rmt_item32_t*)malloc(maxDataLen*sizeof(rmt_item32_t));
|
||||
|
||||
|
||||
rmt_config_t config;
|
||||
// Configure the RMT channel for TX
|
||||
bzero(&config, sizeof(rmt_config_t));
|
||||
@@ -153,10 +123,20 @@ RMTChannel::RMTChannel(pinpair pins, bool isMain) {
|
||||
config.channel = channel = (rmt_channel_t)ch;
|
||||
config.clk_div = RMT_CLOCK_DIVIDER;
|
||||
config.gpio_num = (gpio_num_t)pins.pin;
|
||||
config.mem_block_num = RMT_CHAN_PER_DCC_CHAN;
|
||||
// use config
|
||||
config.mem_block_num = 2; // With longest DCC packet 11 inc checksum (future expansion)
|
||||
// number of bits needed is 22preamble + start +
|
||||
// 11*9 + extrazero + EOT = 124
|
||||
// 2 mem block of 64 RMT items should be enough
|
||||
|
||||
ESP_ERROR_CHECK(rmt_config(&config));
|
||||
addPin(pins.invpin, true);
|
||||
/*
|
||||
// test: config another gpio pin
|
||||
gpio_num_t gpioNum = (gpio_num_t)(pin-1);
|
||||
PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[gpioNum], PIN_FUNC_GPIO);
|
||||
gpio_set_direction(gpioNum, GPIO_MODE_OUTPUT);
|
||||
gpio_matrix_out(gpioNum, RMT_SIG_OUT0_IDX, 0, 0);
|
||||
*/
|
||||
|
||||
// NOTE: ESP_INTR_FLAG_IRAM is *NOT* included in this bitmask
|
||||
ESP_ERROR_CHECK(rmt_driver_install(config.channel, 0, ESP_INTR_FLAG_LOWMED|ESP_INTR_FLAG_SHARED));
|
||||
|
@@ -3,6 +3,7 @@
|
||||
* © 2021 Mike S
|
||||
* © 2021-2023 Harald Barth
|
||||
* © 2021 Fred Decker
|
||||
* © 2023 Travis Farmer
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
@@ -90,6 +91,8 @@ private:
|
||||
static const int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle
|
||||
#if defined(ARDUINO_ARCH_STM32) // TODO: PMA temporary hack - assumes 100Mhz F_CPU as STM32 can change frequency
|
||||
static const long CLOCK_CYCLES=(100000000L / 1000000 * DCC_SIGNAL_TIME) >>1;
|
||||
#elif defined(ARDUINO_GIGA)
|
||||
static const long CLOCK_CYCLES=(480000000L / 1000000 * DCC_SIGNAL_TIME) >>1;
|
||||
#else
|
||||
static const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME) >>1;
|
||||
#endif
|
||||
|
206
DCCTimerGiga.cpp
Normal file
206
DCCTimerGiga.cpp
Normal file
@@ -0,0 +1,206 @@
|
||||
/*
|
||||
* © 2023 Travis Farmer
|
||||
* © 2023 Neil McKechnie
|
||||
* © 2022-2023 Paul M. Antoine
|
||||
* © 2021 Mike S
|
||||
* © 2021, 2023 Harald Barth
|
||||
* © 2021 Fred Decker
|
||||
* © 2021 Chris Harlow
|
||||
* © 2021 David Cutting
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of Asbelos DCC 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/>.
|
||||
*/
|
||||
|
||||
// ATTENTION: this file only compiles on a STM32 based boards
|
||||
// Please refer to DCCTimer.h for general comments about how this class works
|
||||
// This is to avoid repetition and duplication.
|
||||
#if defined(ARDUINO_GIGA)
|
||||
|
||||
#include "DCCTimer.h"
|
||||
#include "DIAG.h"
|
||||
#include "GigaHardwareTimer.h"
|
||||
#include <Arduino_AdvancedAnalog.h>
|
||||
//#include "config.h"
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// Experimental code for High Accuracy (HA) DCC Signal mode
|
||||
// Warning - use of TIM2 and TIM3 can affect the use of analogWrite() function on certain pins,
|
||||
// which is used by the DC motor types.
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
INTERRUPT_CALLBACK interruptHandler=0;
|
||||
|
||||
//HardwareTimer* timer = NULL;
|
||||
//HardwareTimer* timerAux = NULL;
|
||||
HardwareTimer timer(TIM2);
|
||||
HardwareTimer timerAux(TIM3);
|
||||
|
||||
static bool tim2ModeHA = false;
|
||||
static bool tim3ModeHA = false;
|
||||
|
||||
void DCCTimer_Handler() __attribute__((interrupt));
|
||||
|
||||
void DCCTimer_Handler() {
|
||||
interruptHandler();
|
||||
}
|
||||
|
||||
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||
interruptHandler=callback;
|
||||
noInterrupts();
|
||||
|
||||
// adc_set_sample_rate(ADC_SAMPLETIME_480CYCLES);
|
||||
timer.pause();
|
||||
timerAux.pause();
|
||||
timer.setPrescaleFactor(1);
|
||||
timer.setOverflow(DCC_SIGNAL_TIME, MICROSEC_FORMAT);
|
||||
timer.attachInterrupt(DCCTimer_Handler);
|
||||
timer.refresh();
|
||||
timerAux.setPrescaleFactor(1);
|
||||
timerAux.setOverflow(DCC_SIGNAL_TIME, MICROSEC_FORMAT);
|
||||
timerAux.refresh();
|
||||
|
||||
timer.resume();
|
||||
timerAux.resume();
|
||||
|
||||
interrupts();
|
||||
}
|
||||
|
||||
bool DCCTimer::isPWMPin(byte pin) {
|
||||
switch (pin) {
|
||||
case 12:
|
||||
return true;
|
||||
case 13:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void DCCTimer::setPWM(byte pin, bool high) {
|
||||
switch (pin) {
|
||||
case 12:
|
||||
if (!tim3ModeHA) {
|
||||
timerAux.setMode(1, TIMER_OUTPUT_COMPARE_INACTIVE, 12);
|
||||
tim3ModeHA = true;
|
||||
}
|
||||
if (high)
|
||||
TIM2->CCMR1 = (TIM2->CCMR1 & ~TIM_CCMR1_OC1M_Msk) | TIM_CCMR1_OC1M_0;
|
||||
else
|
||||
TIM2->CCMR1 = (TIM2->CCMR1 & ~TIM_CCMR1_OC1M_Msk) | TIM_CCMR1_OC1M_1;
|
||||
break;
|
||||
case 13:
|
||||
if (!tim2ModeHA) {
|
||||
timer.setMode(1, TIMER_OUTPUT_COMPARE_INACTIVE, 13);
|
||||
tim2ModeHA = true;
|
||||
}
|
||||
if (high)
|
||||
TIM3->CCMR1 = (TIM3->CCMR1 & ~TIM_CCMR1_OC1M_Msk) | TIM_CCMR1_OC1M_0;
|
||||
else
|
||||
TIM3->CCMR1 = (TIM3->CCMR1 & ~TIM_CCMR1_OC1M_Msk) | TIM_CCMR1_OC1M_1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void DCCTimer::clearPWM() {
|
||||
timer.setMode(1, TIMER_OUTPUT_COMPARE_INACTIVE, NC);
|
||||
tim2ModeHA = false;
|
||||
timerAux.setMode(1, TIMER_OUTPUT_COMPARE_INACTIVE, NC);
|
||||
tim3ModeHA = false;
|
||||
}
|
||||
|
||||
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||
volatile uint32_t *serno1 = (volatile uint32_t *)UID_BASE;
|
||||
volatile uint32_t *serno2 = (volatile uint32_t *)UID_BASE+4;
|
||||
volatile uint32_t *serno3 = (volatile uint32_t *)UID_BASE+8;
|
||||
volatile uint32_t m1 = *serno1;
|
||||
volatile uint32_t m2 = *serno2;
|
||||
volatile uint32_t m3 = *serno3;
|
||||
mac[0] = 0xBE;
|
||||
mac[1] = 0xEF;
|
||||
mac[2] = m1 ^ m3 >> 24;
|
||||
mac[3] = m1 ^ m3 >> 16;
|
||||
mac[4] = m1 ^ m3 >> 8;
|
||||
mac[5] = m1 ^ m3 >> 0;
|
||||
//DIAG(F("MAC: %P:%P:%P:%P:%P:%P"),mac[0],mac[1],mac[2],mac[3],mac[4],mac[5]);
|
||||
|
||||
}
|
||||
volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
|
||||
|
||||
// Return low memory value...
|
||||
int DCCTimer::getMinimumFreeMemory() {
|
||||
noInterrupts(); // Disable interrupts to get volatile value
|
||||
int retval = freeMemory();
|
||||
interrupts();
|
||||
return retval;
|
||||
}
|
||||
extern "C" char* sbrk(int incr);
|
||||
|
||||
int DCCTimer::freeMemory() {
|
||||
|
||||
char top;
|
||||
unsigned int tmp = (unsigned int)(&top - reinterpret_cast<char*>(sbrk(0)));
|
||||
return (int)(tmp / 1000);
|
||||
}
|
||||
|
||||
void DCCTimer::reset() {
|
||||
//Watchdog &watchdog = Watchdog::get_instance();
|
||||
//Watchdog::stop();
|
||||
//Watchdog::start(500);
|
||||
|
||||
//while(true) {};
|
||||
}
|
||||
|
||||
int * ADCee::analogvals = NULL;
|
||||
|
||||
int16_t ADCee::ADCmax()
|
||||
{
|
||||
return 1023;
|
||||
}
|
||||
|
||||
AdvancedADC adc(A0, A1);
|
||||
int ADCee::init(uint8_t pin) {
|
||||
adc.begin(AN_RESOLUTION_10, 16000, 1, 512);
|
||||
return 123;
|
||||
}
|
||||
|
||||
/*
|
||||
* Read function ADCee::read(pin) to get value instead of analogRead(pin)
|
||||
*/
|
||||
int ADCee::read(uint8_t pin, bool fromISR) {
|
||||
static SampleBuffer buf = adc.read();
|
||||
int retVal = -123;
|
||||
if (adc.available()) {
|
||||
buf.release();
|
||||
buf = adc.read();
|
||||
}
|
||||
return (buf[pin - A0]);
|
||||
}
|
||||
|
||||
/*
|
||||
* Scan function that is called from interrupt
|
||||
*/
|
||||
#pragma GCC push_options
|
||||
#pragma GCC optimize ("-O3")
|
||||
void ADCee::scan() {
|
||||
}
|
||||
#pragma GCC pop_options
|
||||
|
||||
void ADCee::begin() {
|
||||
noInterrupts();
|
||||
|
||||
interrupts();
|
||||
}
|
||||
#endif
|
@@ -50,16 +50,11 @@ HardwareSerial Serial6(PA12, PA11); // Rx=PA12, Tx=PA11 -- CN10 pins 12 and 14
|
||||
// via the debugger on the Nucleo-64. It is therefore unavailable for other DCC-EX uses like WiFi, DFPlayer, etc.
|
||||
// On the F446RE, Serial3 and Serial5 are easy to use:
|
||||
HardwareSerial Serial3(PC11, PC10); // Rx=PC11, Tx=PC10 -- USART3 - F446RE
|
||||
HardwareSerial Serial5(PD2, PC12); // Rx=PD2, Tx=PC12 -- UART5 - F446RE
|
||||
HardwareSerial Serial5(PD2, PC12); // Rx=PC7, Tx=PC6 -- 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)
|
||||
#elif defined(ARDUINO_NUCLEO_F412ZG) || defined(ARDUINO_NUCLEO_F413ZH) || defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F446ZE)
|
||||
// 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
|
||||
#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.
|
||||
#else
|
||||
@@ -220,9 +215,9 @@ void DCCTimer::clearPWM() {
|
||||
}
|
||||
|
||||
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||
volatile uint32_t *serno1 = (volatile uint32_t *)UID_BASE;
|
||||
volatile uint32_t *serno2 = (volatile uint32_t *)UID_BASE+4;
|
||||
// volatile uint32_t *serno3 = (volatile uint32_t *)UID_BASE+8;
|
||||
volatile uint32_t *serno1 = (volatile uint32_t *)0x1FFF7A10;
|
||||
volatile uint32_t *serno2 = (volatile uint32_t *)0x1FFF7A14;
|
||||
// volatile uint32_t *serno3 = (volatile uint32_t *)0x1FFF7A18;
|
||||
|
||||
volatile uint32_t m1 = *serno1;
|
||||
volatile uint32_t m2 = *serno2;
|
||||
|
@@ -106,7 +106,6 @@ void DCCWaveform::interruptHandler() {
|
||||
DCCWaveform::DCCWaveform( byte preambleBits, bool isMain) {
|
||||
isMainTrack = isMain;
|
||||
packetPending = false;
|
||||
reminderWindowOpen = false;
|
||||
memcpy(transmitPacket, idlePacket, sizeof(idlePacket));
|
||||
state = WAVE_START;
|
||||
// The +1 below is to allow the preamble generator to create the stop bit
|
||||
@@ -128,15 +127,9 @@ void DCCWaveform::interrupt2() {
|
||||
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();
|
||||
// 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);
|
||||
DCCTimer::updateMinimumFreeMemoryISR(22);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -155,9 +148,30 @@ void DCCWaveform::interrupt2() {
|
||||
if (bytes_sent >= transmitLength) {
|
||||
// end of transmission buffer... repeat or switch to next message
|
||||
bytes_sent = 0;
|
||||
// preamble for next packet will start...
|
||||
remainingPreambles = requiredPreambles;
|
||||
|
||||
if (transmitRepeats > 0) {
|
||||
transmitRepeats--;
|
||||
}
|
||||
else if (packetPending) {
|
||||
// Copy pending packet to transmit packet
|
||||
// a fixed length memcpy is faster than a variable length loop for these small lengths
|
||||
// for (int b = 0; b < pendingLength; b++) transmitPacket[b] = pendingPacket[b];
|
||||
memcpy( transmitPacket, pendingPacket, sizeof(pendingPacket));
|
||||
|
||||
transmitLength = pendingLength;
|
||||
transmitRepeats = pendingRepeats;
|
||||
packetPending = false;
|
||||
clearResets();
|
||||
}
|
||||
else {
|
||||
// Fortunately reset and idle packets are the same length
|
||||
memcpy( transmitPacket, isMainTrack ? idlePacket : resetPacket, sizeof(idlePacket));
|
||||
transmitLength = sizeof(idlePacket);
|
||||
transmitRepeats = 0;
|
||||
if (getResets() < 250) sentResetsSincePacket++; // only place to increment (private!)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#pragma GCC pop_options
|
||||
@@ -179,39 +193,8 @@ void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repea
|
||||
packetPending = true;
|
||||
clearResets();
|
||||
}
|
||||
|
||||
bool DCCWaveform::isReminderWindowOpen() {
|
||||
return reminderWindowOpen && ! packetPending;
|
||||
}
|
||||
|
||||
void DCCWaveform::promotePendingPacket() {
|
||||
// fill the transmission packet from the pending packet
|
||||
|
||||
// Just keep going if repeating
|
||||
if (transmitRepeats > 0) {
|
||||
transmitRepeats--;
|
||||
return;
|
||||
}
|
||||
|
||||
if (packetPending) {
|
||||
// Copy pending packet to transmit packet
|
||||
// a fixed length memcpy is faster than a variable length loop for these small lengths
|
||||
// for (int b = 0; b < pendingLength; b++) transmitPacket[b] = pendingPacket[b];
|
||||
memcpy( transmitPacket, pendingPacket, sizeof(pendingPacket));
|
||||
|
||||
transmitLength = pendingLength;
|
||||
transmitRepeats = pendingRepeats;
|
||||
packetPending = false;
|
||||
clearResets();
|
||||
return;
|
||||
}
|
||||
|
||||
// nothing to do, just send idles or resets
|
||||
// Fortunately reset and idle packets are the same length
|
||||
memcpy( transmitPacket, isMainTrack ? idlePacket : resetPacket, sizeof(idlePacket));
|
||||
transmitLength = sizeof(idlePacket);
|
||||
transmitRepeats = 0;
|
||||
if (getResets() < 250) sentResetsSincePacket++; // only place to increment (private!)
|
||||
bool DCCWaveform::getPacketPending() {
|
||||
return packetPending;
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -283,15 +266,15 @@ void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repea
|
||||
}
|
||||
}
|
||||
|
||||
bool DCCWaveform::isReminderWindowOpen() {
|
||||
bool DCCWaveform::getPacketPending() {
|
||||
if(isMainTrack) {
|
||||
if (rmtMainChannel == NULL)
|
||||
return false;
|
||||
return !rmtMainChannel->busy();
|
||||
return true;
|
||||
return rmtMainChannel->busy();
|
||||
} else {
|
||||
if (rmtProgChannel == NULL)
|
||||
return false;
|
||||
return !rmtProgChannel->busy();
|
||||
return true;
|
||||
return rmtProgChannel->busy();
|
||||
}
|
||||
}
|
||||
void IRAM_ATTR DCCWaveform::loop() {
|
||||
|
@@ -2,7 +2,7 @@
|
||||
* © 2021 M Steve Todd
|
||||
* © 2021 Mike S
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2024 Harald Barth
|
||||
* © 2020-2021 Harald Barth
|
||||
* © 2020-2021 Chris Harlow
|
||||
* All rights reserved.
|
||||
*
|
||||
@@ -33,9 +33,9 @@
|
||||
|
||||
|
||||
// Number of preamble bits.
|
||||
const byte PREAMBLE_BITS_MAIN = 16;
|
||||
const byte PREAMBLE_BITS_PROG = 22;
|
||||
const byte MAX_PACKET_SIZE = 5; // NMRA standard extended packets, payload size WITHOUT checksum.
|
||||
const int PREAMBLE_BITS_MAIN = 16;
|
||||
const int PREAMBLE_BITS_PROG = 22;
|
||||
const byte MAX_PACKET_SIZE = 5; // NMRA standard extended packets, payload size WITHOUT checksum.
|
||||
|
||||
|
||||
// The WAVE_STATE enum is deliberately numbered because a change of order would be catastrophic
|
||||
@@ -76,13 +76,11 @@ class DCCWaveform {
|
||||
};
|
||||
#endif
|
||||
void schedulePacket(const byte buffer[], byte byteCount, byte repeats);
|
||||
bool isReminderWindowOpen();
|
||||
void promotePendingPacket();
|
||||
bool getPacketPending();
|
||||
|
||||
private:
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
volatile bool packetPending;
|
||||
volatile bool reminderWindowOpen;
|
||||
volatile byte sentResetsSincePacket;
|
||||
#else
|
||||
volatile uint32_t resetPacketBase;
|
||||
|
@@ -37,9 +37,7 @@
|
||||
class Display : public DisplayInterface {
|
||||
public:
|
||||
Display(DisplayDevice *deviceDriver);
|
||||
#if !defined (MAX_CHARACTER_ROWS)
|
||||
static const int MAX_CHARACTER_ROWS = 8;
|
||||
#endif
|
||||
static const int MAX_CHARACTER_COLS = MAX_MSG_SIZE;
|
||||
static const long DISPLAY_SCROLL_TIME = 3000; // 3 seconds
|
||||
|
||||
|
@@ -54,9 +54,7 @@
|
||||
xxx; \
|
||||
t->refresh();}
|
||||
#else
|
||||
#define DISPLAY_START(xxx) { \
|
||||
xxx; \
|
||||
}
|
||||
#define DISPLAY_START(xxx) {}
|
||||
|
||||
#endif
|
||||
#endif // LCD_Implementation_h
|
||||
|
@@ -31,12 +31,12 @@
|
||||
#include "Sensors.h"
|
||||
#include "Turnouts.h"
|
||||
|
||||
#if defined(ARDUINO_ARCH_SAMC)
|
||||
#if defined(ARDUINO_ARCH_SAMC) || defined(ARDUINO_GIGA)
|
||||
ExternalEEPROM EEPROM;
|
||||
#endif
|
||||
|
||||
void EEStore::init() {
|
||||
#if defined(ARDUINO_ARCH_SAMC)
|
||||
#if defined(ARDUINO_ARCH_SAMC) || defined(ARDUINO_GIGA)
|
||||
EEPROM.begin(0x50); // Address for Microchip 24-series EEPROM with all three
|
||||
// A pins grounded (0b1010000 = 0x50)
|
||||
#endif
|
||||
|
@@ -26,7 +26,7 @@
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#if defined(ARDUINO_ARCH_SAMC)
|
||||
#if defined(ARDUINO_ARCH_SAMC) || defined(ARDUINO_GIGA)
|
||||
#include <SparkFun_External_EEPROM.h>
|
||||
extern ExternalEEPROM EEPROM;
|
||||
#else
|
||||
|
436
EXRAIL2.cpp
436
EXRAIL2.cpp
@@ -55,6 +55,22 @@
|
||||
#include "Turntables.h"
|
||||
#include "IODevice.h"
|
||||
|
||||
// Command parsing keywords
|
||||
const int16_t HASH_KEYWORD_EXRAIL=15435;
|
||||
const int16_t HASH_KEYWORD_ON = 2657;
|
||||
const int16_t HASH_KEYWORD_START=23232;
|
||||
const int16_t HASH_KEYWORD_RESERVE=11392;
|
||||
const int16_t HASH_KEYWORD_FREE=-23052;
|
||||
const int16_t HASH_KEYWORD_LATCH=1618;
|
||||
const int16_t HASH_KEYWORD_UNLATCH=1353;
|
||||
const int16_t HASH_KEYWORD_PAUSE=-4142;
|
||||
const int16_t HASH_KEYWORD_RESUME=27609;
|
||||
const int16_t HASH_KEYWORD_KILL=5218;
|
||||
const int16_t HASH_KEYWORD_ALL=3457;
|
||||
const int16_t HASH_KEYWORD_ROUTES=-3702;
|
||||
const int16_t HASH_KEYWORD_RED=26099;
|
||||
const int16_t HASH_KEYWORD_AMBER=18713;
|
||||
const int16_t HASH_KEYWORD_GREEN=-31493;
|
||||
|
||||
// One instance of RMFT clas is used for each "thread" in the automation.
|
||||
// Each thread manages a loco on a journey through the layout, and/or may manage a scenery automation.
|
||||
@@ -70,7 +86,7 @@ RMFT2 * RMFT2::pausingTask=NULL; // Task causing a PAUSE.
|
||||
// and all others will have their locos stopped, then resumed after the pausing task resumes.
|
||||
byte RMFT2::flags[MAX_FLAGS];
|
||||
Print * RMFT2::LCCSerial=0;
|
||||
LookList * RMFT2::routeLookup=NULL;
|
||||
LookList * RMFT2::sequenceLookup=NULL;
|
||||
LookList * RMFT2::onThrowLookup=NULL;
|
||||
LookList * RMFT2::onCloseLookup=NULL;
|
||||
LookList * RMFT2::onActivateLookup=NULL;
|
||||
@@ -84,10 +100,9 @@ LookList * RMFT2::onClockLookup=NULL;
|
||||
LookList * RMFT2::onRotateLookup=NULL;
|
||||
#endif
|
||||
LookList * RMFT2::onOverloadLookup=NULL;
|
||||
byte * RMFT2::routeStateArray=nullptr;
|
||||
const FSH * * RMFT2::routeCaptionArray=nullptr;
|
||||
int16_t * RMFT2::stashArray=nullptr;
|
||||
int16_t RMFT2::maxStashId=0;
|
||||
|
||||
#define GET_OPCODE GETHIGHFLASH(RMFT2::RouteCode,progCounter)
|
||||
#define SKIPOP progCounter+=3
|
||||
|
||||
// getOperand instance version, uses progCounter from instance.
|
||||
uint16_t RMFT2::getOperand(byte n) {
|
||||
@@ -105,7 +120,6 @@ uint16_t RMFT2::getOperand(int progCounter,byte n) {
|
||||
LookList::LookList(int16_t size) {
|
||||
m_size=size;
|
||||
m_loaded=0;
|
||||
m_chain=nullptr;
|
||||
if (size) {
|
||||
m_lookupArray=new int16_t[size];
|
||||
m_resultArray=new int16_t[size];
|
||||
@@ -123,35 +137,8 @@ int16_t LookList::find(int16_t value) {
|
||||
for (int16_t i=0;i<m_size;i++) {
|
||||
if (m_lookupArray[i]==value) return m_resultArray[i];
|
||||
}
|
||||
return m_chain ? m_chain->find(value) :-1;
|
||||
}
|
||||
void LookList::chain(LookList * chain) {
|
||||
m_chain=chain;
|
||||
}
|
||||
void LookList::handleEvent(const FSH* reason,int16_t id) {
|
||||
// New feature... create multiple ONhandlers
|
||||
for (int i=0;i<m_size;i++)
|
||||
if (m_lookupArray[i]==id)
|
||||
RMFT2::startNonRecursiveTask(reason,id,m_resultArray[i]);
|
||||
}
|
||||
|
||||
|
||||
void LookList::stream(Print * _stream) {
|
||||
for (int16_t i=0;i<m_size;i++) {
|
||||
_stream->print(" ");
|
||||
_stream->print(m_lookupArray[i]);
|
||||
}
|
||||
}
|
||||
|
||||
int16_t LookList::findPosition(int16_t value) {
|
||||
for (int16_t i=0;i<m_size;i++) {
|
||||
if (m_lookupArray[i]==value) return i;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
int16_t LookList::size() {
|
||||
return m_size;
|
||||
}
|
||||
|
||||
LookList* RMFT2::LookListLoader(OPCODE op1, OPCODE op2, OPCODE op3) {
|
||||
int progCounter;
|
||||
@@ -184,12 +171,7 @@ LookList* RMFT2::LookListLoader(OPCODE op1, OPCODE op2, OPCODE op3) {
|
||||
for (int f=0;f<MAX_FLAGS;f++) flags[f]=0;
|
||||
|
||||
// create lookups
|
||||
routeLookup=LookListLoader(OPCODE_ROUTE, OPCODE_AUTOMATION);
|
||||
routeLookup->chain(LookListLoader(OPCODE_SEQUENCE));
|
||||
if (compileFeatures && FEATURE_ROUTESTATE) {
|
||||
routeStateArray=(byte *)calloc(routeLookup->size(),sizeof(byte));
|
||||
routeCaptionArray=(const FSH * *)calloc(routeLookup->size(),sizeof(const FSH *));
|
||||
}
|
||||
sequenceLookup=LookListLoader(OPCODE_ROUTE, OPCODE_AUTOMATION,OPCODE_SEQUENCE);
|
||||
onThrowLookup=LookListLoader(OPCODE_ONTHROW);
|
||||
onCloseLookup=LookListLoader(OPCODE_ONCLOSE);
|
||||
onActivateLookup=LookListLoader(OPCODE_ONACTIVATE);
|
||||
@@ -234,12 +216,6 @@ if (compileFeatures & FEATURE_SIGNAL) {
|
||||
IODevice::configureInput((VPIN)pin,true);
|
||||
break;
|
||||
}
|
||||
case OPCODE_STASH:
|
||||
case OPCODE_CLEAR_STASH:
|
||||
case OPCODE_PICKUP_STASH: {
|
||||
maxStashId=max(maxStashId,((int16_t)operand));
|
||||
break;
|
||||
}
|
||||
|
||||
case OPCODE_ATGTE:
|
||||
case OPCODE_ATLT:
|
||||
@@ -319,42 +295,257 @@ if (compileFeatures & FEATURE_SIGNAL) {
|
||||
}
|
||||
}
|
||||
SKIPOP; // include ENDROUTES opcode
|
||||
|
||||
if (compileFeatures & FEATURE_STASH) {
|
||||
// create the stash array from the highest id found
|
||||
if (maxStashId>0) stashArray=(int16_t*)calloc(maxStashId+1, sizeof(int16_t));
|
||||
//TODO check EEPROM and fetch stashArray
|
||||
}
|
||||
|
||||
DIAG(F("EXRAIL %db, fl=%d, stash=%d"),progCounter,MAX_FLAGS, maxStashId);
|
||||
|
||||
DIAG(F("EXRAIL %db, fl=%d"),progCounter,MAX_FLAGS);
|
||||
|
||||
// Removed for 4.2.31 new RMFT2(0); // add the startup route
|
||||
diag=saved_diag;
|
||||
}
|
||||
|
||||
void RMFT2::setTurnoutHiddenState(Turnout * t) {
|
||||
// turnout descriptions are in low flash F strings
|
||||
const FSH *desc = getTurnoutDescription(t->getId());
|
||||
if (desc) t->setHidden(GETFLASH(desc)==0x01);
|
||||
// turnout descriptions are in low flash F strings
|
||||
t->setHidden(GETFLASH(getTurnoutDescription(t->getId()))==0x01);
|
||||
}
|
||||
|
||||
#ifndef IO_NO_HAL
|
||||
void RMFT2::setTurntableHiddenState(Turntable * tto) {
|
||||
const FSH *desc = getTurntableDescription(tto->getId());
|
||||
if (desc) tto->setHidden(GETFLASH(desc)==0x01);
|
||||
tto->setHidden(GETFLASH(getTurntableDescription(tto->getId()))==0x01);
|
||||
}
|
||||
#endif
|
||||
|
||||
char RMFT2::getRouteType(int16_t id) {
|
||||
int16_t progCounter=routeLookup->find(id);
|
||||
if (progCounter>=0) {
|
||||
byte type=GET_OPCODE;
|
||||
if (type==OPCODE_ROUTE) return 'R';
|
||||
if (type==OPCODE_AUTOMATION) return 'A';
|
||||
for (int16_t i=0;;i+=2) {
|
||||
int16_t rid= GETHIGHFLASHW(routeIdList,i);
|
||||
if (rid==INT16_MAX) break;
|
||||
if (rid==id) return 'R';
|
||||
}
|
||||
for (int16_t i=0;;i+=2) {
|
||||
int16_t rid= GETHIGHFLASHW(automationIdList,i);
|
||||
if (rid==INT16_MAX) break;
|
||||
if (rid==id) return 'A';
|
||||
}
|
||||
return 'X';
|
||||
}
|
||||
|
||||
// This filter intercepts <> commands to do the following:
|
||||
// - Implement RMFT specific commands/diagnostics
|
||||
// - Reject/modify JMRI commands that would interfere with RMFT processing
|
||||
void RMFT2::ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16_t p[]) {
|
||||
(void)stream; // avoid compiler warning if we don't access this parameter
|
||||
bool reject=false;
|
||||
switch(opcode) {
|
||||
|
||||
case 'D':
|
||||
if (p[0]==HASH_KEYWORD_EXRAIL) { // <D EXRAIL ON/OFF>
|
||||
diag = paramCount==2 && (p[1]==HASH_KEYWORD_ON || p[1]==1);
|
||||
opcode=0;
|
||||
}
|
||||
break;
|
||||
|
||||
case '/': // New EXRAIL command
|
||||
reject=!parseSlash(stream,paramCount,p);
|
||||
opcode=0;
|
||||
break;
|
||||
case 'L':
|
||||
if (compileFeatures & FEATURE_LCC) {
|
||||
// This entire code block is compiled out if LLC macros not used
|
||||
if (paramCount==0) { //<L> LCC adapter introducing self
|
||||
LCCSerial=stream; // now we know where to send events we raise
|
||||
|
||||
// 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"),
|
||||
getOperand(progCounter,1),
|
||||
getOperand(progCounter,2),
|
||||
getOperand(progCounter,3),
|
||||
getOperand(progCounter,0)
|
||||
);
|
||||
}}
|
||||
|
||||
// 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...
|
||||
StringFormatter::send(stream,F("<LL %d x%h%h%h:%h>\n"),
|
||||
eventIndex,
|
||||
getOperand(progCounter,1),
|
||||
getOperand(progCounter,2),
|
||||
getOperand(progCounter,3),
|
||||
getOperand(progCounter,0)
|
||||
);
|
||||
eventIndex++;
|
||||
}
|
||||
}
|
||||
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];
|
||||
reject=eventid<0 || eventid>=countLCCLookup;
|
||||
if (!reject) startNonRecursiveTask(F("LCC"),eventid,onLCCLookup[eventid]);
|
||||
opcode=0;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default: // other commands pass through
|
||||
break;
|
||||
}
|
||||
if (reject) {
|
||||
opcode=0;
|
||||
StringFormatter::send(stream,F("<X>\n"));
|
||||
}
|
||||
}
|
||||
|
||||
bool RMFT2::parseSlash(Print * stream, byte & paramCount, int16_t p[]) {
|
||||
|
||||
if (paramCount==0) { // STATUS
|
||||
StringFormatter::send(stream, F("<* EXRAIL STATUS"));
|
||||
RMFT2 * task=loopTask;
|
||||
while(task) {
|
||||
StringFormatter::send(stream,F("\nID=%d,PC=%d,LOCO=%d%c,SPEED=%d%c"),
|
||||
(int)(task->taskId),task->progCounter,task->loco,
|
||||
task->invert?'I':' ',
|
||||
task->speedo,
|
||||
task->forward?'F':'R'
|
||||
);
|
||||
task=task->next;
|
||||
if (task==loopTask) break;
|
||||
}
|
||||
// Now stream the flags
|
||||
for (int id=0;id<MAX_FLAGS; id++) {
|
||||
byte flag=flags[id];
|
||||
if (flag & ~TASK_FLAG & ~SIGNAL_MASK) { // not interested in TASK_FLAG only. Already shown above
|
||||
StringFormatter::send(stream,F("\nflags[%d] "),id);
|
||||
if (flag & SECTION_FLAG) StringFormatter::send(stream,F(" RESERVED"));
|
||||
if (flag & LATCH_FLAG) StringFormatter::send(stream,F(" LATCHED"));
|
||||
}
|
||||
}
|
||||
|
||||
if (compileFeatures & FEATURE_SIGNAL) {
|
||||
// do the signals
|
||||
// flags[n] represents the state of the nth signal in the table
|
||||
for (int sigslot=0;;sigslot++) {
|
||||
VPIN sigid=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigslot*8);
|
||||
if (sigid==0) break; // end of signal list
|
||||
byte flag=flags[sigslot] & SIGNAL_MASK; // obtain signal flags for this id
|
||||
StringFormatter::send(stream,F("\n%S[%d]"),
|
||||
(flag == SIGNAL_RED)? F("RED") : (flag==SIGNAL_GREEN) ? F("GREEN") : F("AMBER"),
|
||||
sigid & SIGNAL_ID_MASK);
|
||||
}
|
||||
}
|
||||
StringFormatter::send(stream,F(" *>\n"));
|
||||
return true;
|
||||
}
|
||||
switch (p[0]) {
|
||||
case HASH_KEYWORD_PAUSE: // </ PAUSE>
|
||||
if (paramCount!=1) return false;
|
||||
DCC::setThrottle(0,1,true); // pause all locos on the track
|
||||
pausingTask=(RMFT2 *)1; // Impossible task address
|
||||
return true;
|
||||
|
||||
case HASH_KEYWORD_RESUME: // </ RESUME>
|
||||
if (paramCount!=1) return false;
|
||||
pausingTask=NULL;
|
||||
{
|
||||
RMFT2 * task=loopTask;
|
||||
while(task) {
|
||||
if (task->loco) task->driveLoco(task->speedo);
|
||||
task=task->next;
|
||||
if (task==loopTask) break;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
|
||||
|
||||
case HASH_KEYWORD_START: // </ START [cab] route >
|
||||
if (paramCount<2 || paramCount>3) return false;
|
||||
{
|
||||
int route=(paramCount==2) ? p[1] : p[2];
|
||||
uint16_t cab=(paramCount==2)? 0 : p[1];
|
||||
int pc=sequenceLookup->find(route);
|
||||
if (pc<0) return false;
|
||||
RMFT2* task=new RMFT2(pc);
|
||||
task->loco=cab;
|
||||
}
|
||||
return true;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// check KILL ALL here, otherwise the next validation confuses ALL with a flag
|
||||
if (p[0]==HASH_KEYWORD_KILL && p[1]==HASH_KEYWORD_ALL) {
|
||||
while (loopTask) loopTask->kill(F("KILL ALL")); // destructor changes loopTask
|
||||
return true;
|
||||
}
|
||||
|
||||
// all other / commands take 1 parameter
|
||||
if (paramCount!=2 ) return false;
|
||||
|
||||
switch (p[0]) {
|
||||
case HASH_KEYWORD_KILL: // Kill taskid|ALL
|
||||
{
|
||||
if ( p[1]<0 || p[1]>=MAX_FLAGS) return false;
|
||||
RMFT2 * task=loopTask;
|
||||
while(task) {
|
||||
if (task->taskId==p[1]) {
|
||||
task->kill(F("KILL"));
|
||||
return true;
|
||||
}
|
||||
task=task->next;
|
||||
if (task==loopTask) break;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
|
||||
case HASH_KEYWORD_RESERVE: // force reserve a section
|
||||
return setFlag(p[1],SECTION_FLAG);
|
||||
|
||||
case HASH_KEYWORD_FREE: // force free a section
|
||||
return setFlag(p[1],0,SECTION_FLAG);
|
||||
|
||||
case HASH_KEYWORD_LATCH:
|
||||
return setFlag(p[1], LATCH_FLAG);
|
||||
|
||||
case HASH_KEYWORD_UNLATCH:
|
||||
return setFlag(p[1], 0, LATCH_FLAG);
|
||||
|
||||
case HASH_KEYWORD_RED:
|
||||
doSignal(p[1],SIGNAL_RED);
|
||||
return true;
|
||||
|
||||
case HASH_KEYWORD_AMBER:
|
||||
doSignal(p[1],SIGNAL_AMBER);
|
||||
return true;
|
||||
|
||||
case HASH_KEYWORD_GREEN:
|
||||
doSignal(p[1],SIGNAL_GREEN);
|
||||
return true;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// This emits Routes and Automations to Withrottle
|
||||
// Automations are given a state to set the button to "handoff" which implies
|
||||
// handing over the loco to the automation.
|
||||
// Routes are given "Set" buttons and do not cause the loco to be handed over.
|
||||
|
||||
|
||||
|
||||
RMFT2::RMFT2(int progCtr) {
|
||||
progCounter=progCtr;
|
||||
@@ -403,7 +594,7 @@ RMFT2::~RMFT2() {
|
||||
}
|
||||
|
||||
void RMFT2::createNewTask(int route, uint16_t cab) {
|
||||
int pc=routeLookup->find(route);
|
||||
int pc=sequenceLookup->find(route);
|
||||
if (pc<0) return;
|
||||
RMFT2* task=new RMFT2(pc);
|
||||
task->loco=cab;
|
||||
@@ -416,6 +607,7 @@ void RMFT2::driveLoco(byte speed) {
|
||||
power on appropriate track if DC or main if dcc
|
||||
if (TrackManager::getMainPowerMode()==POWERMODE::OFF) {
|
||||
TrackManager::setMainPower(POWERMODE::ON);
|
||||
CommandDistributor::broadcastPower();
|
||||
}
|
||||
**********/
|
||||
|
||||
@@ -643,6 +835,7 @@ void RMFT2::loop2() {
|
||||
case OPCODE_POWEROFF:
|
||||
TrackManager::setPower(POWERMODE::OFF);
|
||||
TrackManager::setJoin(false);
|
||||
CommandDistributor::broadcastPower();
|
||||
break;
|
||||
|
||||
case OPCODE_SET_POWER:
|
||||
@@ -650,10 +843,10 @@ void RMFT2::loop2() {
|
||||
//byte thistrack=getOperand(1);
|
||||
switch (operand) {
|
||||
case TRACK_POWER_0:
|
||||
TrackManager::setTrackPower(POWERMODE::OFF, getOperand(1));
|
||||
TrackManager::setTrackPower(TrackManager::isProg(getOperand(1)), false, POWERMODE::OFF, getOperand(1));
|
||||
break;
|
||||
case TRACK_POWER_1:
|
||||
TrackManager::setTrackPower(POWERMODE::ON, getOperand(1));
|
||||
TrackManager::setTrackPower(TrackManager::isProg(getOperand(1)), false, POWERMODE::ON, getOperand(1));
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -664,7 +857,7 @@ void RMFT2::loop2() {
|
||||
// If DC/DCX use my loco for DC address
|
||||
{
|
||||
TRACK_MODE mode = (TRACK_MODE)(operand>>8);
|
||||
int16_t cab=(mode & TRACK_MODE_DC) ? loco : 0;
|
||||
int16_t cab=(mode==TRACK_MODE_DC || mode==TRACK_MODE_DCX) ? loco : 0;
|
||||
TrackManager::setTrackMode(operand & 0x0F, mode, cab);
|
||||
}
|
||||
break;
|
||||
@@ -802,7 +995,7 @@ void RMFT2::loop2() {
|
||||
}
|
||||
|
||||
case OPCODE_FOLLOW:
|
||||
progCounter=routeLookup->find(operand);
|
||||
progCounter=sequenceLookup->find(operand);
|
||||
if (progCounter<0) kill(F("FOLLOW unknown"), operand);
|
||||
return;
|
||||
|
||||
@@ -812,7 +1005,7 @@ void RMFT2::loop2() {
|
||||
return;
|
||||
}
|
||||
callStack[stackDepth++]=progCounter+3;
|
||||
progCounter=routeLookup->find(operand);
|
||||
progCounter=sequenceLookup->find(operand);
|
||||
if (progCounter<0) kill(F("CALL unknown"),operand);
|
||||
return;
|
||||
|
||||
@@ -837,10 +1030,12 @@ void RMFT2::loop2() {
|
||||
case OPCODE_JOIN:
|
||||
TrackManager::setPower(POWERMODE::ON);
|
||||
TrackManager::setJoin(true);
|
||||
CommandDistributor::broadcastPower();
|
||||
break;
|
||||
|
||||
case OPCODE_UNJOIN:
|
||||
TrackManager::setJoin(false);
|
||||
CommandDistributor::broadcastPower();
|
||||
break;
|
||||
|
||||
case OPCODE_READ_LOCO1: // READ_LOCO is implemented as 2 separate opcodes
|
||||
@@ -868,11 +1063,12 @@ void RMFT2::loop2() {
|
||||
case OPCODE_POWERON:
|
||||
TrackManager::setMainPower(POWERMODE::ON);
|
||||
TrackManager::setJoin(false);
|
||||
CommandDistributor::broadcastPower();
|
||||
break;
|
||||
|
||||
case OPCODE_START:
|
||||
{
|
||||
int newPc=routeLookup->find(operand);
|
||||
int newPc=sequenceLookup->find(operand);
|
||||
if (newPc<0) break;
|
||||
new RMFT2(newPc);
|
||||
}
|
||||
@@ -880,7 +1076,7 @@ void RMFT2::loop2() {
|
||||
|
||||
case OPCODE_SENDLOCO: // cab, route
|
||||
{
|
||||
int newPc=routeLookup->find(getOperand(1));
|
||||
int newPc=sequenceLookup->find(getOperand(1));
|
||||
if (newPc<0) break;
|
||||
RMFT2* newtask=new RMFT2(newPc); // create new task
|
||||
newtask->loco=operand;
|
||||
@@ -934,47 +1130,7 @@ void RMFT2::loop2() {
|
||||
case OPCODE_PRINT:
|
||||
printMessage(operand);
|
||||
break;
|
||||
case OPCODE_ROUTE_HIDDEN:
|
||||
manageRouteState(operand,2);
|
||||
break;
|
||||
case OPCODE_ROUTE_INACTIVE:
|
||||
manageRouteState(operand,0);
|
||||
break;
|
||||
case OPCODE_ROUTE_ACTIVE:
|
||||
manageRouteState(operand,1);
|
||||
break;
|
||||
case OPCODE_ROUTE_DISABLED:
|
||||
manageRouteState(operand,4);
|
||||
break;
|
||||
|
||||
case OPCODE_STASH:
|
||||
if (compileFeatures & FEATURE_STASH)
|
||||
stashArray[operand] = invert? -loco : loco;
|
||||
break;
|
||||
|
||||
case OPCODE_CLEAR_STASH:
|
||||
if (compileFeatures & FEATURE_STASH)
|
||||
stashArray[operand] = 0;
|
||||
break;
|
||||
|
||||
case OPCODE_CLEAR_ALL_STASH:
|
||||
if (compileFeatures & FEATURE_STASH)
|
||||
for (int i=0;i<=maxStashId;i++) stashArray[operand]=0;
|
||||
break;
|
||||
|
||||
case OPCODE_PICKUP_STASH:
|
||||
if (compileFeatures & FEATURE_STASH) {
|
||||
int16_t x=stashArray[operand];
|
||||
if (x>=0) {
|
||||
loco=x;
|
||||
invert=false;
|
||||
break;
|
||||
}
|
||||
loco=-x;
|
||||
invert=true;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case OPCODE_ROUTE:
|
||||
case OPCODE_AUTOMATION:
|
||||
case OPCODE_SEQUENCE:
|
||||
@@ -1062,9 +1218,9 @@ int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
|
||||
// Schedule any event handler for this signal change.
|
||||
// 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);
|
||||
if (rag==SIGNAL_RED) handleEvent(F("RED"),onRedLookup,id);
|
||||
else if (rag==SIGNAL_GREEN) handleEvent(F("GREEN"), onGreenLookup,id);
|
||||
else handleEvent(F("AMBER"), onAmberLookup,id);
|
||||
|
||||
int16_t sigslot=getSignalSlot(id);
|
||||
if (sigslot<0) return;
|
||||
@@ -1133,26 +1289,26 @@ int16_t RMFT2::getSignalSlot(int16_t id) {
|
||||
|
||||
void RMFT2::turnoutEvent(int16_t turnoutId, bool closed) {
|
||||
// Hunt for an ONTHROW/ONCLOSE for this turnout
|
||||
if (closed) onCloseLookup->handleEvent(F("CLOSE"),turnoutId);
|
||||
else onThrowLookup->handleEvent(F("THROW"),turnoutId);
|
||||
if (closed) handleEvent(F("CLOSE"),onCloseLookup,turnoutId);
|
||||
else handleEvent(F("THROW"),onThrowLookup,turnoutId);
|
||||
}
|
||||
|
||||
|
||||
void RMFT2::activateEvent(int16_t addr, bool activate) {
|
||||
// Hunt for an ONACTIVATE/ONDEACTIVATE for this accessory
|
||||
if (activate) onActivateLookup->handleEvent(F("ACTIVATE"),addr);
|
||||
else onDeactivateLookup->handleEvent(F("DEACTIVATE"),addr);
|
||||
if (activate) handleEvent(F("ACTIVATE"),onActivateLookup,addr);
|
||||
else handleEvent(F("DEACTIVATE"),onDeactivateLookup,addr);
|
||||
}
|
||||
|
||||
void RMFT2::changeEvent(int16_t vpin, bool change) {
|
||||
// Hunt for an ONCHANGE for this sensor
|
||||
if (change) onChangeLookup->handleEvent(F("CHANGE"),vpin);
|
||||
if (change) handleEvent(F("CHANGE"),onChangeLookup,vpin);
|
||||
}
|
||||
|
||||
#ifndef IO_NO_HAL
|
||||
void RMFT2::rotateEvent(int16_t turntableId, bool change) {
|
||||
// Hunt or an ONROTATE for this turntable
|
||||
if (change) onRotateLookup->handleEvent(F("ROTATE"),turntableId);
|
||||
if (change) handleEvent(F("ROTATE"),onRotateLookup,turntableId);
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -1161,8 +1317,8 @@ void RMFT2::clockEvent(int16_t clocktime, bool change) {
|
||||
if (Diag::CMD)
|
||||
DIAG(F("Looking for clock event at : %d"), clocktime);
|
||||
if (change) {
|
||||
onClockLookup->handleEvent(F("CLOCK"),clocktime);
|
||||
onClockLookup->handleEvent(F("CLOCK"),25*60+clocktime%60);
|
||||
handleEvent(F("CLOCK"),onClockLookup,clocktime);
|
||||
handleEvent(F("CLOCK"),onClockLookup,25*60+clocktime%60);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1171,10 +1327,16 @@ void RMFT2::powerEvent(int16_t track, bool overload) {
|
||||
if (Diag::CMD)
|
||||
DIAG(F("Looking for Power event on track : %c"), track);
|
||||
if (overload) {
|
||||
onOverloadLookup->handleEvent(F("POWER"),track);
|
||||
handleEvent(F("POWER"),onOverloadLookup,track);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void RMFT2::handleEvent(const FSH* reason,LookList* handlers, int16_t id) {
|
||||
int pc= handlers->find(id);
|
||||
if (pc>=0) startNonRecursiveTask(reason,id,pc);
|
||||
}
|
||||
|
||||
void RMFT2::startNonRecursiveTask(const FSH* reason, int16_t id,int pc) {
|
||||
// Check we dont already have a task running this handler
|
||||
RMFT2 * task=loopTask;
|
||||
@@ -1291,29 +1453,3 @@ void RMFT2::thrungeString(uint32_t strfar, thrunger mode, byte id) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RMFT2::manageRouteState(uint16_t id, byte state) {
|
||||
if (compileFeatures && FEATURE_ROUTESTATE) {
|
||||
// Route state must be maintained for when new throttles connect.
|
||||
// locate route id in the Routes lookup
|
||||
int16_t position=routeLookup->findPosition(id);
|
||||
if (position<0) return;
|
||||
// set state beside it
|
||||
if (routeStateArray[position]==state) return;
|
||||
routeStateArray[position]=state;
|
||||
CommandDistributor::broadcastRouteState(id,state);
|
||||
}
|
||||
}
|
||||
void RMFT2::manageRouteCaption(uint16_t id,const FSH* caption) {
|
||||
if (compileFeatures && FEATURE_ROUTESTATE) {
|
||||
// Route state must be maintained for when new throttles connect.
|
||||
// locate route id in the Routes lookup
|
||||
int16_t position=routeLookup->findPosition(id);
|
||||
if (position<0) return;
|
||||
// set state beside it
|
||||
if (routeCaptionArray[position]==caption) return;
|
||||
routeCaptionArray[position]=caption;
|
||||
CommandDistributor::broadcastRouteCaption(id,caption);
|
||||
}
|
||||
}
|
||||
|
||||
|
35
EXRAIL2.h
35
EXRAIL2.h
@@ -68,9 +68,6 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
|
||||
OPCODE_ONROTATE,OPCODE_ROTATE,OPCODE_WAITFORTT,
|
||||
OPCODE_LCC,OPCODE_LCCX,OPCODE_ONLCC,
|
||||
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,
|
||||
|
||||
// OPcodes below this point are skip-nesting IF operations
|
||||
// placed here so that they may be skipped as a group
|
||||
@@ -102,8 +99,6 @@ enum thrunger: byte {
|
||||
static const byte FEATURE_SIGNAL= 0x80;
|
||||
static const byte FEATURE_LCC = 0x40;
|
||||
static const byte FEATURE_ROSTER= 0x20;
|
||||
static const byte FEATURE_ROUTESTATE= 0x10;
|
||||
static const byte FEATURE_STASH = 0x08;
|
||||
|
||||
|
||||
// Flag bits for status of hardware and TPL
|
||||
@@ -124,20 +119,13 @@ enum thrunger: byte {
|
||||
class LookList {
|
||||
public:
|
||||
LookList(int16_t size);
|
||||
void chain(LookList* chainTo);
|
||||
void add(int16_t lookup, int16_t result);
|
||||
int16_t find(int16_t value); // finds result value
|
||||
int16_t findPosition(int16_t value); // finds index
|
||||
int16_t size();
|
||||
void stream(Print * _stream);
|
||||
void handleEvent(const FSH* reason,int16_t id);
|
||||
|
||||
int16_t find(int16_t value);
|
||||
private:
|
||||
int16_t m_size;
|
||||
int16_t m_loaded;
|
||||
int16_t * m_lookupArray;
|
||||
int16_t * m_resultArray;
|
||||
LookList* m_chain;
|
||||
int16_t * m_resultArray;
|
||||
};
|
||||
|
||||
class RMFT2 {
|
||||
@@ -171,8 +159,7 @@ class LookList {
|
||||
static const FSH * getRosterFunctions(int16_t id);
|
||||
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[]) ;
|
||||
@@ -189,7 +176,9 @@ private:
|
||||
#endif
|
||||
static LookList* LookListLoader(OPCODE op1,
|
||||
OPCODE op2=OPCODE_ENDEXRAIL,OPCODE op3=OPCODE_ENDEXRAIL);
|
||||
static void handleEvent(const FSH* reason,LookList* handlers, int16_t id);
|
||||
static uint16_t getOperand(int progCounter,byte n);
|
||||
static void startNonRecursiveTask(const FSH* reason, int16_t id,int pc);
|
||||
static RMFT2 * loopTask;
|
||||
static RMFT2 * pausingTask;
|
||||
void delayMe(long millisecs);
|
||||
@@ -205,11 +194,11 @@ private:
|
||||
uint16_t getOperand(byte n);
|
||||
|
||||
static bool diag;
|
||||
static const HIGHFLASH3 byte RouteCode[];
|
||||
static const HIGHFLASH byte RouteCode[];
|
||||
static const HIGHFLASH int16_t SignalDefinitions[];
|
||||
static byte flags[MAX_FLAGS];
|
||||
static Print * LCCSerial;
|
||||
static LookList * routeLookup;
|
||||
static LookList * sequenceLookup;
|
||||
static LookList * onThrowLookup;
|
||||
static LookList * onCloseLookup;
|
||||
static LookList * onActivateLookup;
|
||||
@@ -227,12 +216,6 @@ private:
|
||||
static const int countLCCLookup;
|
||||
static int onLCCLookup[];
|
||||
static const byte compileFeatures;
|
||||
static void manageRouteState(uint16_t id, byte state);
|
||||
static void manageRouteCaption(uint16_t id, const FSH* caption);
|
||||
static byte * routeStateArray;
|
||||
static const FSH ** routeCaptionArray;
|
||||
static int16_t * stashArray;
|
||||
static int16_t maxStashId;
|
||||
|
||||
// Local variables - exist for each instance/task
|
||||
RMFT2 *next; // loop chain
|
||||
@@ -254,8 +237,4 @@ private:
|
||||
byte stackDepth;
|
||||
int callStack[MAX_STACK_DEPTH];
|
||||
};
|
||||
|
||||
#define GET_OPCODE GETHIGHFLASH(RMFT2::RouteCode,progCounter)
|
||||
#define SKIPOP progCounter+=3
|
||||
|
||||
#endif
|
||||
|
@@ -39,8 +39,6 @@
|
||||
#undef AUTOSTART
|
||||
#undef BROADCAST
|
||||
#undef CALL
|
||||
#undef CLEAR_STASH
|
||||
#undef CLEAR_ALL_STASH
|
||||
#undef CLOSE
|
||||
#undef DCC_SIGNAL
|
||||
#undef DCC_TURNTABLE
|
||||
@@ -67,7 +65,6 @@
|
||||
#undef FWD
|
||||
#undef GREEN
|
||||
#undef HAL
|
||||
#undef HAL_IGNORE_DEFAULTS
|
||||
#undef IF
|
||||
#undef IFAMBER
|
||||
#undef IFCLOSED
|
||||
@@ -111,7 +108,6 @@
|
||||
#undef ONCHANGE
|
||||
#undef PARSE
|
||||
#undef PAUSE
|
||||
#undef PICKUP_STASH
|
||||
#undef PIN_TURNOUT
|
||||
#undef PRINT
|
||||
#ifndef DISABLE_PROG
|
||||
@@ -130,11 +126,6 @@
|
||||
#undef ROTATE
|
||||
#undef ROTATE_DCC
|
||||
#undef ROUTE
|
||||
#undef ROUTE_ACTIVE
|
||||
#undef ROUTE_INACTIVE
|
||||
#undef ROUTE_HIDDEN
|
||||
#undef ROUTE_DISABLED
|
||||
#undef ROUTE_CAPTION
|
||||
#undef SENDLOCO
|
||||
#undef SEQUENCE
|
||||
#undef SERIAL
|
||||
@@ -156,8 +147,6 @@
|
||||
#undef SIGNALH
|
||||
#undef SPEED
|
||||
#undef START
|
||||
#undef STASH
|
||||
#undef STEALTH
|
||||
#undef STOP
|
||||
#undef THROW
|
||||
#undef TT_ADDPOSITION
|
||||
@@ -190,9 +179,7 @@
|
||||
#define AUTOMATION(id,description)
|
||||
#define AUTOSTART
|
||||
#define BROADCAST(msg)
|
||||
#define CALL(route)
|
||||
#define CLEAR_STASH(id)
|
||||
#define CLEAR_ALL_STASH(id)
|
||||
#define CALL(route)
|
||||
#define CLOSE(id)
|
||||
#define DCC_SIGNAL(id,add,subaddr)
|
||||
#define DCC_TURNTABLE(id,home,description)
|
||||
@@ -219,7 +206,6 @@
|
||||
#define FWD(speed)
|
||||
#define GREEN(signal_id)
|
||||
#define HAL(haltype,params...)
|
||||
#define HAL_IGNORE_DEFAULTS
|
||||
#define IF(sensor_id)
|
||||
#define IFAMBER(signal_id)
|
||||
#define IFCLOSED(turnout_id)
|
||||
@@ -265,7 +251,6 @@
|
||||
#define PIN_TURNOUT(id,pin,description...)
|
||||
#define PRINT(msg)
|
||||
#define PARSE(msg)
|
||||
#define PICKUP_STASH(id)
|
||||
#ifndef DISABLE_PROG
|
||||
#define POM(cv,value)
|
||||
#endif
|
||||
@@ -282,11 +267,6 @@
|
||||
#define ROTATE_DCC(turntable_id,position)
|
||||
#define ROSTER(cab,name,funcmap...)
|
||||
#define ROUTE(id,description)
|
||||
#define ROUTE_ACTIVE(id)
|
||||
#define ROUTE_INACTIVE(id)
|
||||
#define ROUTE_HIDDEN(id)
|
||||
#define ROUTE_DISABLED(id)
|
||||
#define ROUTE_CAPTION(id,caption)
|
||||
#define SENDLOCO(cab,route)
|
||||
#define SEQUENCE(id)
|
||||
#define SERIAL(msg)
|
||||
@@ -307,9 +287,7 @@
|
||||
#define SIGNAL(redpin,amberpin,greenpin)
|
||||
#define SIGNALH(redpin,amberpin,greenpin)
|
||||
#define SPEED(speed)
|
||||
#define START(route)
|
||||
#define STASH(id)
|
||||
#define STEALTH(code...)
|
||||
#define START(route)
|
||||
#define STOP
|
||||
#define THROW(id)
|
||||
#define TT_ADDPOSITION(turntable_id,position,value,angle,description...)
|
||||
|
@@ -1,310 +0,0 @@
|
||||
/*
|
||||
* © 2021 Neil McKechnie
|
||||
* © 2021-2023 Harald Barth
|
||||
* © 2020-2023 Chris Harlow
|
||||
* © 2022-2023 Colin Murdoch
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
// THIS file is an extension of the RMFT2 class
|
||||
// normally found in EXRAIL2.cpp
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "defines.h"
|
||||
#include "EXRAIL2.h"
|
||||
#include "DCC.h"
|
||||
#include "KeywordHasher.h"
|
||||
|
||||
// This filter intercepts <> commands to do the following:
|
||||
// - Implement RMFT specific commands/diagnostics
|
||||
// - Reject/modify JMRI commands that would interfere with RMFT processing
|
||||
|
||||
void RMFT2::ComandFilter(Print * stream, byte & opcode, byte & paramCount, int16_t p[]) {
|
||||
(void)stream; // avoid compiler warning if we don't access this parameter
|
||||
bool reject=false;
|
||||
switch(opcode) {
|
||||
|
||||
case 'D':
|
||||
if (p[0]=="EXRAIL"_hk) { // <D EXRAIL ON/OFF>
|
||||
diag = paramCount==2 && (p[1]=="ON"_hk || p[1]==1);
|
||||
opcode=0;
|
||||
}
|
||||
break;
|
||||
|
||||
case '/': // New EXRAIL command
|
||||
reject=!parseSlash(stream,paramCount,p);
|
||||
opcode=0;
|
||||
break;
|
||||
|
||||
case 'L':
|
||||
// This entire code block is compiled out if LLC macros not used
|
||||
if (!(compileFeatures & FEATURE_LCC)) return;
|
||||
|
||||
if (paramCount==0) { //<L> LCC adapter introducing self
|
||||
LCCSerial=stream; // now we know where to send events we raise
|
||||
|
||||
// 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"),
|
||||
getOperand(progCounter,1),
|
||||
getOperand(progCounter,2),
|
||||
getOperand(progCounter,3),
|
||||
getOperand(progCounter,0)
|
||||
);
|
||||
}}
|
||||
|
||||
// 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...
|
||||
StringFormatter::send(stream,F("<LL %d x%h%h%h:%h>\n"),
|
||||
eventIndex,
|
||||
getOperand(progCounter,1),
|
||||
getOperand(progCounter,2),
|
||||
getOperand(progCounter,3),
|
||||
getOperand(progCounter,0)
|
||||
);
|
||||
eventIndex++;
|
||||
}
|
||||
}
|
||||
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];
|
||||
reject=eventid<0 || eventid>=countLCCLookup;
|
||||
if (!reject) startNonRecursiveTask(F("LCC"),eventid,onLCCLookup[eventid]);
|
||||
opcode=0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'J': // throttle info commands
|
||||
if (paramCount<1) return;
|
||||
switch(p[0]) {
|
||||
case "A"_hk: // <JA> returns automations/routes
|
||||
if (paramCount==1) {// <JA>
|
||||
StringFormatter::send(stream, F("<jA"));
|
||||
routeLookup->stream(stream);
|
||||
StringFormatter::send(stream, F(">\n"));
|
||||
opcode=0;
|
||||
return;
|
||||
}
|
||||
if (paramCount==2) { // <JA id>
|
||||
int16_t id=p[1];
|
||||
StringFormatter::send(stream,F("<jA %d %c \"%S\">\n"),
|
||||
id, getRouteType(id), getRouteDescription(id));
|
||||
|
||||
if (compileFeatures & FEATURE_ROUTESTATE) {
|
||||
// Send any non-default button states or captions
|
||||
int16_t statePos=routeLookup->findPosition(id);
|
||||
if (statePos>=0) {
|
||||
if (routeStateArray[statePos])
|
||||
StringFormatter::send(stream,F("<jB %d %d>\n"), id, routeStateArray[statePos]);
|
||||
if (routeCaptionArray[statePos])
|
||||
StringFormatter::send(stream,F("<jB %d \"%S\">\n"), id,routeCaptionArray[statePos]);
|
||||
}
|
||||
}
|
||||
opcode=0;
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case "M"_hk:
|
||||
// NOTE: we only need to handle valid calls here because
|
||||
// DCCEXParser has to have code to handle the <J<> cases where
|
||||
// exrail isnt involved anyway.
|
||||
// This entire code block is compiled out if STASH macros not used
|
||||
if (!(compileFeatures & FEATURE_STASH)) return;
|
||||
if (paramCount==1) { // <JM>
|
||||
StringFormatter::send(stream,F("<jM %d>\n"),maxStashId);
|
||||
opcode=0;
|
||||
break;
|
||||
}
|
||||
if (paramCount==2) { // <JM id>
|
||||
if (p[1]<=0 || p[1]>maxStashId) break;
|
||||
StringFormatter::send(stream,F("<jM %d %d>\n"),
|
||||
p[1],stashArray[p[1]]);
|
||||
opcode=0;
|
||||
break;
|
||||
}
|
||||
if (paramCount==3) { // <JM id cab>
|
||||
if (p[1]<=0 || p[1]>maxStashId) break;
|
||||
stashArray[p[1]]=p[2];
|
||||
opcode=0;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
default: // other commands pass through
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool RMFT2::parseSlash(Print * stream, byte & paramCount, int16_t p[]) {
|
||||
|
||||
if (paramCount==0) { // STATUS
|
||||
StringFormatter::send(stream, F("<* EXRAIL STATUS"));
|
||||
RMFT2 * task=loopTask;
|
||||
while(task) {
|
||||
StringFormatter::send(stream,F("\nID=%d,PC=%d,LOCO=%d%c,SPEED=%d%c"),
|
||||
(int)(task->taskId),task->progCounter,task->loco,
|
||||
task->invert?'I':' ',
|
||||
task->speedo,
|
||||
task->forward?'F':'R'
|
||||
);
|
||||
task=task->next;
|
||||
if (task==loopTask) break;
|
||||
}
|
||||
// Now stream the flags
|
||||
for (int id=0;id<MAX_FLAGS; id++) {
|
||||
byte flag=flags[id];
|
||||
if (flag & ~TASK_FLAG & ~SIGNAL_MASK) { // not interested in TASK_FLAG only. Already shown above
|
||||
StringFormatter::send(stream,F("\nflags[%d] "),id);
|
||||
if (flag & SECTION_FLAG) StringFormatter::send(stream,F(" RESERVED"));
|
||||
if (flag & LATCH_FLAG) StringFormatter::send(stream,F(" LATCHED"));
|
||||
}
|
||||
}
|
||||
|
||||
if (compileFeatures & FEATURE_SIGNAL) {
|
||||
// do the signals
|
||||
// flags[n] represents the state of the nth signal in the table
|
||||
for (int sigslot=0;;sigslot++) {
|
||||
VPIN sigid=GETHIGHFLASHW(RMFT2::SignalDefinitions,sigslot*8);
|
||||
if (sigid==0) break; // end of signal list
|
||||
byte flag=flags[sigslot] & SIGNAL_MASK; // obtain signal flags for this id
|
||||
StringFormatter::send(stream,F("\n%S[%d]"),
|
||||
(flag == SIGNAL_RED)? F("RED") : (flag==SIGNAL_GREEN) ? F("GREEN") : F("AMBER"),
|
||||
sigid & SIGNAL_ID_MASK);
|
||||
}
|
||||
}
|
||||
|
||||
if (compileFeatures & FEATURE_STASH) {
|
||||
for (int i=1;i<=maxStashId;i++) {
|
||||
if (stashArray[i])
|
||||
StringFormatter::send(stream,F("\nSTASH[%d] Loco=%d"),
|
||||
i, stashArray[i]);
|
||||
}
|
||||
}
|
||||
|
||||
StringFormatter::send(stream,F(" *>\n"));
|
||||
return true;
|
||||
}
|
||||
switch (p[0]) {
|
||||
case "PAUSE"_hk: // </ PAUSE>
|
||||
if (paramCount!=1) return false;
|
||||
DCC::setThrottle(0,1,true); // pause all locos on the track
|
||||
pausingTask=(RMFT2 *)1; // Impossible task address
|
||||
return true;
|
||||
|
||||
case "RESUME"_hk: // </ RESUME>
|
||||
if (paramCount!=1) return false;
|
||||
pausingTask=NULL;
|
||||
{
|
||||
RMFT2 * task=loopTask;
|
||||
while(task) {
|
||||
if (task->loco) task->driveLoco(task->speedo);
|
||||
task=task->next;
|
||||
if (task==loopTask) break;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
|
||||
|
||||
case "START"_hk: // </ START [cab] route >
|
||||
if (paramCount<2 || paramCount>3) return false;
|
||||
{
|
||||
int route=(paramCount==2) ? p[1] : p[2];
|
||||
uint16_t cab=(paramCount==2)? 0 : p[1];
|
||||
int pc=routeLookup->find(route);
|
||||
if (pc<0) return false;
|
||||
RMFT2* task=new RMFT2(pc);
|
||||
task->loco=cab;
|
||||
}
|
||||
return true;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// check KILL ALL here, otherwise the next validation confuses ALL with a flag
|
||||
if (p[0]=="KILL"_hk && p[1]=="ALL"_hk) {
|
||||
while (loopTask) loopTask->kill(F("KILL ALL")); // destructor changes loopTask
|
||||
return true;
|
||||
}
|
||||
|
||||
// all other / commands take 1 parameter
|
||||
if (paramCount!=2 ) return false;
|
||||
|
||||
switch (p[0]) {
|
||||
case "KILL"_hk: // Kill taskid|ALL
|
||||
{
|
||||
if ( p[1]<0 || p[1]>=MAX_FLAGS) return false;
|
||||
RMFT2 * task=loopTask;
|
||||
while(task) {
|
||||
if (task->taskId==p[1]) {
|
||||
task->kill(F("KILL"));
|
||||
return true;
|
||||
}
|
||||
task=task->next;
|
||||
if (task==loopTask) break;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
|
||||
case "RESERVE"_hk: // force reserve a section
|
||||
return setFlag(p[1],SECTION_FLAG);
|
||||
|
||||
case "FREE"_hk: // force free a section
|
||||
return setFlag(p[1],0,SECTION_FLAG);
|
||||
|
||||
case "LATCH"_hk:
|
||||
return setFlag(p[1], LATCH_FLAG);
|
||||
|
||||
case "UNLATCH"_hk:
|
||||
return setFlag(p[1], 0, LATCH_FLAG);
|
||||
|
||||
case "RED"_hk:
|
||||
doSignal(p[1],SIGNAL_RED);
|
||||
return true;
|
||||
|
||||
case "AMBER"_hk:
|
||||
doSignal(p[1],SIGNAL_AMBER);
|
||||
return true;
|
||||
|
||||
case "GREEN"_hk:
|
||||
doSignal(p[1],SIGNAL_GREEN);
|
||||
return true;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
114
EXRAILMacros.h
114
EXRAILMacros.h
@@ -74,81 +74,13 @@
|
||||
#define ALIAS(name,value...) const int name= 1##value##0 ==10 ? -__COUNTER__ : value##0/10;
|
||||
#include "myAutomation.h"
|
||||
|
||||
// Pass 1d Detect sequence duplicates.
|
||||
// This pass generates no runtime data or code
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef AUTOMATION
|
||||
#define AUTOMATION(id, description) id,
|
||||
#undef ROUTE
|
||||
#define ROUTE(id, description) id,
|
||||
#undef SEQUENCE
|
||||
#define SEQUENCE(id) id,
|
||||
constexpr int16_t compileTimeSequenceList[]={
|
||||
#include "myAutomation.h"
|
||||
0
|
||||
};
|
||||
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 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 uint16_t pos ) {
|
||||
return pos>=stuffSize? false :
|
||||
compileTimeSequenceList[pos]==value
|
||||
|| hasseq(value,pos+1)
|
||||
|| hasdup(compileTimeSequenceList[pos],pos+1);
|
||||
}
|
||||
|
||||
|
||||
static_assert(!hasdup(compileTimeSequenceList[0],1),"Duplicate SEQUENCE/ROUTE/AUTOMATION detected");
|
||||
|
||||
//pass 1s static asserts to
|
||||
// - check call and follows etc for existing sequence numbers
|
||||
// - check range on LATCH/UNLATCH
|
||||
// This pass generates no runtime data or code
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef CALL
|
||||
#define CALL(id) static_assert(hasseq(id),"Sequence not found");
|
||||
#undef FOLLOW
|
||||
#define FOLLOW(id) static_assert(hasseq(id),"Sequence not found");
|
||||
#undef START
|
||||
#define START(id) static_assert(hasseq(id),"Sequence not found");
|
||||
#undef SENDLOCO
|
||||
#define SENDLOCO(cab,id) static_assert(hasseq(id),"Sequence not found");
|
||||
#undef LATCH
|
||||
#define LATCH(id) static_assert(id>=0 && id<MAX_FLAGS,"Id out of valid range 0-255" );
|
||||
#undef UNLATCH
|
||||
#define UNLATCH(id) static_assert(id>=0 && id<MAX_FLAGS,"Id out of valid range 0-255" );
|
||||
#undef RESERVE
|
||||
#define RESERVE(id) static_assert(id>=0 && id<MAX_FLAGS,"Id out of valid range 0-255" );
|
||||
#undef FREE
|
||||
#define FREE(id) static_assert(id>=0 && id<MAX_FLAGS,"Id out of valid range 0-255" );
|
||||
#undef SPEED
|
||||
#define SPEED(speed) static_assert(speed>=0 && speed<128,"Speed out of valid range 0-127");
|
||||
#undef FWD
|
||||
#define FWD(speed) static_assert(speed>=0 && speed<128,"Speed out of valid range 0-127");
|
||||
#undef REV
|
||||
#define REV(speed) static_assert(speed>=0 && speed<128,"Speed out of valid range 0-127");
|
||||
|
||||
#include "myAutomation.h"
|
||||
|
||||
// Pass 1h Implements HAL macro by creating exrailHalSetup function
|
||||
// Also allows creating EXTurntable object
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef HAL
|
||||
#define HAL(haltype,params...) haltype::create(params);
|
||||
#undef HAL_IGNORE_DEFAULTS
|
||||
#define HAL_IGNORE_DEFAULTS ignore_defaults=true;
|
||||
bool exrailHalSetup() {
|
||||
bool ignore_defaults=false;
|
||||
void exrailHalSetup() {
|
||||
#include "myAutomation.h"
|
||||
return ignore_defaults;
|
||||
}
|
||||
|
||||
// Pass 1c detect compile time featurtes
|
||||
@@ -170,25 +102,6 @@ bool exrailHalSetup() {
|
||||
#define LCCX(senderid,eventid) | FEATURE_LCC
|
||||
#undef ONLCC
|
||||
#define ONLCC(senderid,eventid) | FEATURE_LCC
|
||||
#undef ROUTE_ACTIVE
|
||||
#define ROUTE_ACTIVE(id) | FEATURE_ROUTESTATE
|
||||
#undef ROUTE_INACTIVE
|
||||
#define ROUTE_INACTIVE(id) | FEATURE_ROUTESTATE
|
||||
#undef ROUTE_HIDDEN
|
||||
#define ROUTE_HIDDEN(id) | FEATURE_ROUTESTATE
|
||||
#undef ROUTE_DISABLED
|
||||
#define ROUTE_DISABLED(id) | FEATURE_ROUTESTATE
|
||||
#undef ROUTE_CAPTION
|
||||
#define ROUTE_CAPTION(id,caption) | FEATURE_ROUTESTATE
|
||||
|
||||
#undef CLEAR_STASH
|
||||
#define CLEAR_STASH(id) | FEATURE_STASH
|
||||
#undef CLEAR_ALL_STASH
|
||||
#define CLEAR_ALL_STASH | FEATURE_STASH
|
||||
#undef PICKUP_STASH
|
||||
#define PICKUP_STASH(id) | FEATURE_STASH
|
||||
#undef STASH
|
||||
#define STASH(id) | FEATURE_STASH
|
||||
|
||||
const byte RMFT2::compileFeatures = 0
|
||||
#include "myAutomation.h"
|
||||
@@ -198,7 +111,7 @@ const byte RMFT2::compileFeatures = 0
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef ROUTE
|
||||
#define ROUTE(id, description) id,
|
||||
const int16_t HIGHFLASH RMFT2::routeIdList[]= {
|
||||
const int16_t HIGHFLASH RMFT2::routeIdList[]= {
|
||||
#include "myAutomation.h"
|
||||
INT16_MAX};
|
||||
// Pass 2a create throttle automation list
|
||||
@@ -240,12 +153,6 @@ const int StringMacroTracker1=__COUNTER__;
|
||||
#define PRINT(msg) THRUNGE(msg,thrunge_print)
|
||||
#undef LCN
|
||||
#define LCN(msg) THRUNGE(msg,thrunge_lcn)
|
||||
#undef ROUTE_CAPTION
|
||||
#define ROUTE_CAPTION(id,caption) \
|
||||
case (__COUNTER__ - StringMacroTracker1) : {\
|
||||
manageRouteCaption(id,F(caption));\
|
||||
return;\
|
||||
}
|
||||
#undef SERIAL
|
||||
#define SERIAL(msg) THRUNGE(msg,thrunge_serial)
|
||||
#undef SERIAL1
|
||||
@@ -278,8 +185,6 @@ case (__COUNTER__ - StringMacroTracker1) : {\
|
||||
lcdid=id;\
|
||||
break;\
|
||||
}
|
||||
#undef STEALTH
|
||||
#define STEALTH(code...) case (__COUNTER__ - StringMacroTracker1) : {code} return;
|
||||
#undef WITHROTTLE
|
||||
#define WITHROTTLE(msg) THRUNGE(msg,thrunge_withrottle)
|
||||
|
||||
@@ -299,8 +204,6 @@ void RMFT2::printMessage(uint16_t id) {
|
||||
#include "EXRAIL2MacroReset.h"
|
||||
#undef TURNOUT
|
||||
#define TURNOUT(id,addr,subaddr,description...) O_DESC(id,description)
|
||||
#undef TURNOUTL
|
||||
#define TURNOUTL(id,addr,description...) O_DESC(id,description)
|
||||
#undef PIN_TURNOUT
|
||||
#define PIN_TURNOUT(id,pin,description...) O_DESC(id,description)
|
||||
#undef SERVO_TURNOUT
|
||||
@@ -432,8 +335,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define AUTOSTART OPCODE_AUTOSTART,0,0,
|
||||
#define BROADCAST(msg) PRINT(msg)
|
||||
#define CALL(route) OPCODE_CALL,V(route),
|
||||
#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),
|
||||
#ifndef IO_NO_HAL
|
||||
#define DCC_TURNTABLE(id,home,description...) OPCODE_DCCTURNTABLE,V(id),OPCODE_PAD,V(home),
|
||||
@@ -464,7 +365,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define FWD(speed) OPCODE_FWD,V(speed),
|
||||
#define GREEN(signal_id) OPCODE_GREEN,V(signal_id),
|
||||
#define HAL(haltype,params...)
|
||||
#define HAL_IGNORE_DEFAULTS
|
||||
#define IF(sensor_id) OPCODE_IF,V(sensor_id),
|
||||
#define IFAMBER(signal_id) OPCODE_IFAMBER,V(signal_id),
|
||||
#define IFCLOSED(turnout_id) OPCODE_IFCLOSED,V(turnout_id),
|
||||
@@ -493,7 +393,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
OPCODE_PAD,V((((uint64_t)sender)>>0)&0xFFFF),
|
||||
#define LCD(id,msg) PRINT(msg)
|
||||
#define SCREEN(display,id,msg) PRINT(msg)
|
||||
#define STEALTH(code...) PRINT(dummy)
|
||||
#define LCN(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),
|
||||
@@ -518,7 +417,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define ONTHROW(turnout_id) OPCODE_ONTHROW,V(turnout_id),
|
||||
#define ONCHANGE(sensor_id) OPCODE_ONCHANGE,V(sensor_id),
|
||||
#define PAUSE OPCODE_PAUSE,0,0,
|
||||
#define PICKUP_STASH(id) OPCODE_PICKUP_STASH,V(id),
|
||||
#define PIN_TURNOUT(id,pin,description...) OPCODE_PINTURNOUT,V(id),OPCODE_PAD,V(pin),
|
||||
#ifndef DISABLE_PROG
|
||||
#define POM(cv,value) OPCODE_POM,V(cv),OPCODE_PAD,V(value),
|
||||
@@ -540,11 +438,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define ROTATE_DCC(id,position) OPCODE_ROTATE,V(id),OPCODE_PAD,V(position),OPCODE_PAD,V(0),
|
||||
#endif
|
||||
#define ROUTE(id, description) OPCODE_ROUTE, V(id),
|
||||
#define ROUTE_ACTIVE(id) OPCODE_ROUTE_ACTIVE,V(id),
|
||||
#define ROUTE_INACTIVE(id) OPCODE_ROUTE_INACTIVE,V(id),
|
||||
#define ROUTE_HIDDEN(id) OPCODE_ROUTE_HIDDEN,V(id),
|
||||
#define ROUTE_DISABLED(id) OPCODE_ROUTE_DISABLED,V(id),
|
||||
#define ROUTE_CAPTION(id,caption) PRINT(caption)
|
||||
#define SENDLOCO(cab,route) OPCODE_SENDLOCO,V(cab),OPCODE_PAD,V(route),
|
||||
#define SEQUENCE(id) OPCODE_SEQUENCE, V(id),
|
||||
#define SERIAL(msg) PRINT(msg)
|
||||
@@ -566,7 +459,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
#define SIGNALH(redpin,amberpin,greenpin)
|
||||
#define SPEED(speed) OPCODE_SPEED,V(speed),
|
||||
#define START(route) OPCODE_START,V(route),
|
||||
#define STASH(id) OPCODE_STASH,V(id),
|
||||
#define STOP OPCODE_SPEED,V(0),
|
||||
#define THROW(id) OPCODE_THROW,V(id),
|
||||
#ifndef IO_NO_HAL
|
||||
@@ -588,7 +480,7 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
|
||||
|
||||
// Build RouteCode
|
||||
const int StringMacroTracker2=__COUNTER__;
|
||||
const HIGHFLASH3 byte RMFT2::RouteCode[] = {
|
||||
const HIGHFLASH byte RMFT2::RouteCode[] = {
|
||||
#include "myAutomation.h"
|
||||
OPCODE_ENDTASK,0,0,OPCODE_ENDEXRAIL,0,0 };
|
||||
|
||||
|
@@ -47,10 +47,6 @@ void EthernetInterface::setup()
|
||||
};
|
||||
|
||||
|
||||
#ifdef IP_ADDRESS
|
||||
static IPAddress myIP(IP_ADDRESS);
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Aquire IP Address from DHCP and start server
|
||||
*
|
||||
@@ -63,15 +59,15 @@ EthernetInterface::EthernetInterface()
|
||||
DCCTimer::getSimulatedMacAddress(mac);
|
||||
connected=false;
|
||||
|
||||
#ifdef IP_ADDRESS
|
||||
Ethernet.begin(mac, myIP);
|
||||
#else
|
||||
#ifdef IP_ADDRESS
|
||||
Ethernet.begin(mac, IP_ADDRESS);
|
||||
#else
|
||||
if (Ethernet.begin(mac) == 0)
|
||||
{
|
||||
DIAG(F("Ethernet.begin FAILED"));
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
if (Ethernet.hardwareStatus() == EthernetNoHardware) {
|
||||
DIAG(F("Ethernet shield not found or W5100"));
|
||||
}
|
||||
@@ -140,7 +136,7 @@ bool EthernetInterface::checkLink() {
|
||||
DIAG(F("Ethernet cable connected"));
|
||||
connected=true;
|
||||
#ifdef IP_ADDRESS
|
||||
Ethernet.setLocalIP(myIP); // for static IP, set it again
|
||||
Ethernet.setLocalIP(IP_ADDRESS); // 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
|
||||
|
3
FSH.h
3
FSH.h
@@ -56,7 +56,6 @@ typedef __FlashStringHelper FSH;
|
||||
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
|
||||
// AVR_MEGA memory deliberately placed at end of link may need _far functions
|
||||
#define HIGHFLASH __attribute__((section(".fini2")))
|
||||
#define HIGHFLASH3 __attribute__((section(".fini3")))
|
||||
#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)
|
||||
@@ -64,7 +63,6 @@ typedef __FlashStringHelper FSH;
|
||||
// AVR_UNO/NANO runtime does not support _far functions so just use _near equivalent
|
||||
// as there is no progmem above 32kb anyway.
|
||||
#define HIGHFLASH PROGMEM
|
||||
#define HIGHFLASH3 PROGMEM
|
||||
#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))
|
||||
@@ -82,7 +80,6 @@ typedef __FlashStringHelper FSH;
|
||||
typedef char FSH;
|
||||
#define FLASH
|
||||
#define HIGHFLASH
|
||||
#define HIGHFLASH3
|
||||
#define GETFARPTR(data) ((uint32_t)(data))
|
||||
#define GETFLASH(addr) (*(const byte *)(addr))
|
||||
#define GETHIGHFLASH(data,offset) (*(const byte *)(GETFARPTR(data)+offset))
|
||||
|
@@ -1 +1 @@
|
||||
#define GITHUB_SHA "devel-202401180721Z"
|
||||
#define GITHUB_SHA "devel-202310230944Z"
|
||||
|
2003
GigaHardwareTimer.cpp
Normal file
2003
GigaHardwareTimer.cpp
Normal file
File diff suppressed because it is too large
Load Diff
220
GigaHardwareTimer.h
Normal file
220
GigaHardwareTimer.h
Normal file
@@ -0,0 +1,220 @@
|
||||
/****************************************************************************************************************************
|
||||
HardwareTimer.h
|
||||
|
||||
For Portenta_H7 boards
|
||||
Written by Khoi Hoang
|
||||
|
||||
Built by Khoi Hoang https://github.com/khoih-prog/Portenta_H7_TimerInterrupt
|
||||
Licensed under MIT license
|
||||
|
||||
Now even you use all these new 16 ISR-based timers,with their maximum interval practically unlimited (limited only by
|
||||
unsigned long miliseconds), you just consume only one Portenta_H7 STM32 timer and avoid conflicting with other cores' tasks.
|
||||
The accuracy is nearly perfect compared to software timers. The most important feature is they're ISR-based timers
|
||||
Therefore, their executions are not blocked by bad-behaving functions / tasks.
|
||||
This important feature is absolutely necessary for mission-critical tasks.
|
||||
|
||||
Version: 1.4.0
|
||||
|
||||
Version Modified By Date Comments
|
||||
------- ----------- ---------- -----------
|
||||
1.2.1 K.Hoang 15/09/2021 Initial coding for Portenta_H7
|
||||
1.3.0 K.Hoang 17/09/2021 Add PWM features and examples
|
||||
1.3.1 K.Hoang 21/09/2021 Fix warnings in PWM examples
|
||||
1.4.0 K.Hoang 22/01/2022 Fix `multiple-definitions` linker error. Fix bug
|
||||
*****************************************************************************************************************************/
|
||||
|
||||
// Modified from stm32 core v2.0.0
|
||||
|
||||
/*
|
||||
Copyright (c) 2017 Daniel Fekete
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
|
||||
Copyright (c) 2019 STMicroelectronics
|
||||
Modified to support Arduino_Core_STM32
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef GIGAHARDWARETIMER_H_
|
||||
#define GIGAHARDWARETIMER_H_
|
||||
#if defined(ARDUINO_GIGA)
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "Gigatimer.h"
|
||||
|
||||
#if defined(HAL_TIM_MODULE_ENABLED) && !defined(HAL_TIM_MODULE_ONLY)
|
||||
|
||||
#define TIMER_CHANNELS 4 // channel5 and channel 6 are not considered here has they don't have gpio output and they don't have interrupt
|
||||
|
||||
typedef enum
|
||||
{
|
||||
TIMER_DISABLED, // == TIM_OCMODE_TIMING no output, useful for only-interrupt
|
||||
// Output Compare
|
||||
TIMER_OUTPUT_COMPARE, // == Obsolete, use TIMER_DISABLED instead. Kept for compatibility reason
|
||||
TIMER_OUTPUT_COMPARE_ACTIVE, // == TIM_OCMODE_ACTIVE pin is set high when counter == channel compare
|
||||
TIMER_OUTPUT_COMPARE_INACTIVE, // == TIM_OCMODE_INACTIVE pin is set low when counter == channel compare
|
||||
TIMER_OUTPUT_COMPARE_TOGGLE, // == TIM_OCMODE_TOGGLE pin toggles when counter == channel compare
|
||||
TIMER_OUTPUT_COMPARE_PWM1, // == TIM_OCMODE_PWM1 pin high when counter < channel compare, low otherwise
|
||||
TIMER_OUTPUT_COMPARE_PWM2, // == TIM_OCMODE_PWM2 pin low when counter < channel compare, high otherwise
|
||||
TIMER_OUTPUT_COMPARE_FORCED_ACTIVE, // == TIM_OCMODE_FORCED_ACTIVE pin always high
|
||||
TIMER_OUTPUT_COMPARE_FORCED_INACTIVE, // == TIM_OCMODE_FORCED_INACTIVE pin always low
|
||||
|
||||
//Input capture
|
||||
TIMER_INPUT_CAPTURE_RISING, // == TIM_INPUTCHANNELPOLARITY_RISING
|
||||
TIMER_INPUT_CAPTURE_FALLING, // == TIM_INPUTCHANNELPOLARITY_FALLING
|
||||
TIMER_INPUT_CAPTURE_BOTHEDGE, // == TIM_INPUTCHANNELPOLARITY_BOTHEDGE
|
||||
|
||||
// Used 2 channels for a single pin. One channel in TIM_INPUTCHANNELPOLARITY_RISING another channel in TIM_INPUTCHANNELPOLARITY_FALLING.
|
||||
// Channels must be used by pair: CH1 with CH2, or CH3 with CH4
|
||||
// This mode is very useful for Frequency and Dutycycle measurement
|
||||
TIMER_INPUT_FREQ_DUTY_MEASUREMENT,
|
||||
|
||||
TIMER_NOT_USED = 0xFFFF // This must be the last item of this enum
|
||||
} TimerModes_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
TICK_FORMAT, // default
|
||||
MICROSEC_FORMAT,
|
||||
HERTZ_FORMAT,
|
||||
} TimerFormat_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
RESOLUTION_1B_COMPARE_FORMAT = 1, // used for Dutycycle: [0 .. 1]
|
||||
RESOLUTION_2B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 3]
|
||||
RESOLUTION_3B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 7]
|
||||
RESOLUTION_4B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 15]
|
||||
RESOLUTION_5B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 31]
|
||||
RESOLUTION_6B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 63]
|
||||
RESOLUTION_7B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 127]
|
||||
RESOLUTION_8B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 255]
|
||||
RESOLUTION_9B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 511]
|
||||
RESOLUTION_10B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 1023]
|
||||
RESOLUTION_11B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 2047]
|
||||
RESOLUTION_12B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 4095]
|
||||
RESOLUTION_13B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 8191]
|
||||
RESOLUTION_14B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 16383]
|
||||
RESOLUTION_15B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 32767]
|
||||
RESOLUTION_16B_COMPARE_FORMAT, // used for Dutycycle: [0 .. 65535]
|
||||
|
||||
TICK_COMPARE_FORMAT = 0x80, // default
|
||||
MICROSEC_COMPARE_FORMAT,
|
||||
HERTZ_COMPARE_FORMAT,
|
||||
PERCENT_COMPARE_FORMAT, // used for Dutycycle
|
||||
} TimerCompareFormat_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include <functional>
|
||||
using callback_function_t = std::function<void(void)>;
|
||||
|
||||
/* Class --------------------------------------------------------*/
|
||||
class HardwareTimer
|
||||
{
|
||||
public:
|
||||
HardwareTimer(TIM_TypeDef *instance);
|
||||
~HardwareTimer(); // destructor
|
||||
|
||||
void pause(void); // Pause counter and all output channels
|
||||
void pauseChannel(uint32_t channel); // Timer is still running but channel (output and interrupt) is disabled
|
||||
void resume(void); // Resume counter and all output channels
|
||||
void resumeChannel(uint32_t channel); // Resume only one channel
|
||||
|
||||
void setPrescaleFactor(uint32_t prescaler); // set prescaler register (which is factor value - 1)
|
||||
uint32_t getPrescaleFactor();
|
||||
|
||||
void setOverflow(uint32_t val, TimerFormat_t format =
|
||||
TICK_FORMAT); // set AutoReload register depending on format provided
|
||||
uint32_t getOverflow(TimerFormat_t format = TICK_FORMAT); // return overflow depending on format provided
|
||||
|
||||
void setPWM(uint32_t channel, PinName pin, uint32_t frequency, uint32_t dutycycle,
|
||||
callback_function_t PeriodCallback = nullptr,
|
||||
callback_function_t CompareCallback = nullptr); // Set all in one command freq in HZ, Duty in percentage. Including both interrup.
|
||||
void setPWM(uint32_t channel, uint32_t pin, uint32_t frequency, uint32_t dutycycle,
|
||||
callback_function_t PeriodCallback = nullptr, callback_function_t CompareCallback = nullptr);
|
||||
|
||||
void setCount(uint32_t val, TimerFormat_t format =
|
||||
TICK_FORMAT); // set timer counter to value 'val' depending on format provided
|
||||
uint32_t getCount(TimerFormat_t format =
|
||||
TICK_FORMAT); // return current counter value of timer depending on format provided
|
||||
|
||||
void setMode(uint32_t channel, TimerModes_t mode,
|
||||
PinName pin = NC); // Configure timer channel with specified mode on specified pin if available
|
||||
void setMode(uint32_t channel, TimerModes_t mode, uint32_t pin);
|
||||
|
||||
TimerModes_t getMode(uint32_t channel); // Retrieve configured mode
|
||||
|
||||
void setPreloadEnable(bool value); // Configure overflow preload enable setting
|
||||
|
||||
uint32_t getCaptureCompare(uint32_t channel,
|
||||
TimerCompareFormat_t format = TICK_COMPARE_FORMAT); // return Capture/Compare register value of specified channel depending on format provided
|
||||
void setCaptureCompare(uint32_t channel, uint32_t compare,
|
||||
TimerCompareFormat_t format = TICK_COMPARE_FORMAT); // set Compare register value of specified channel depending on format provided
|
||||
|
||||
void setInterruptPriority(uint32_t preemptPriority, uint32_t subPriority); // set interrupt priority
|
||||
|
||||
//Add interrupt to period update
|
||||
void attachInterrupt(callback_function_t
|
||||
callback); // Attach interrupt callback which will be called upon update event (timer rollover)
|
||||
void detachInterrupt(); // remove interrupt callback which was attached to update event
|
||||
bool hasInterrupt(); //returns true if a timer rollover interrupt has already been set
|
||||
//Add interrupt to capture/compare channel
|
||||
void attachInterrupt(uint32_t channel,
|
||||
callback_function_t callback); // Attach interrupt callback which will be called upon compare match event of specified channel
|
||||
void detachInterrupt(uint32_t
|
||||
channel); // remove interrupt callback which was attached to compare match event of specified channel
|
||||
bool hasInterrupt(uint32_t channel); //returns true if an interrupt has already been set on the channel compare match
|
||||
void timerHandleDeinit(); // Timer deinitialization
|
||||
|
||||
// Refresh() is usefull while timer is running after some registers update
|
||||
void refresh(
|
||||
void); // Generate update event to force all registers (Autoreload, prescaler, compare) to be taken into account
|
||||
|
||||
uint32_t getTimerClkFreq(); // return timer clock frequency in Hz.
|
||||
|
||||
static void captureCompareCallback(TIM_HandleTypeDef
|
||||
*htim); // Generic Caputre and Compare callback which will call user callback
|
||||
static void updateCallback(TIM_HandleTypeDef
|
||||
*htim); // Generic Update (rollover) callback which will call user callback
|
||||
|
||||
// The following function(s) are available for more advanced timer options
|
||||
TIM_HandleTypeDef *getHandle(); // return the handle address for HAL related configuration
|
||||
int getChannel(uint32_t channel);
|
||||
int getLLChannel(uint32_t channel);
|
||||
int getIT(uint32_t channel);
|
||||
int getAssociatedChannel(uint32_t channel);
|
||||
#if defined(TIM_CCER_CC1NE)
|
||||
bool isComplementaryChannel[TIMER_CHANNELS];
|
||||
#endif
|
||||
private:
|
||||
TimerModes_t _ChannelMode[TIMER_CHANNELS];
|
||||
timerObj_t _timerObj;
|
||||
callback_function_t callbacks[1 +
|
||||
TIMER_CHANNELS]; //Callbacks: 0 for update, 1-4 for channels. (channel5/channel6, if any, doesn't have interrupt)
|
||||
};
|
||||
|
||||
extern timerObj_t *HardwareTimer_Handle[TIMER_NUM];
|
||||
|
||||
extern timer_index_t get_timer_index(TIM_TypeDef *htim);
|
||||
|
||||
#endif /* __cplusplus */
|
||||
|
||||
#endif // HAL_TIM_MODULE_ENABLED && !HAL_TIM_MODULE_ONLY
|
||||
#endif
|
||||
#endif // GIGAHARDWARETIMER_H_
|
950
Gigatimer.c
Normal file
950
Gigatimer.c
Normal file
@@ -0,0 +1,950 @@
|
||||
/****************************************************************************************************************************
|
||||
timer.c
|
||||
|
||||
For Portenta_H7 boards
|
||||
Written by Khoi Hoang
|
||||
|
||||
Built by Khoi Hoang https://github.com/khoih-prog/Portenta_H7_TimerInterrupt
|
||||
Licensed under MIT license
|
||||
|
||||
Now even you use all these new 16 ISR-based timers,with their maximum interval practically unlimited (limited only by
|
||||
unsigned long miliseconds), you just consume only one Portenta_H7 STM32 timer and avoid conflicting with other cores' tasks.
|
||||
The accuracy is nearly perfect compared to software timers. The most important feature is they're ISR-based timers
|
||||
Therefore, their executions are not blocked by bad-behaving functions / tasks.
|
||||
This important feature is absolutely necessary for mission-critical tasks.
|
||||
|
||||
Version: 1.4.0
|
||||
|
||||
Version Modified By Date Comments
|
||||
------- ----------- ---------- -----------
|
||||
1.2.1 K.Hoang 15/09/2021 Initial coding for Portenta_H7
|
||||
1.3.0 K.Hoang 17/09/2021 Add PWM features and examples
|
||||
1.3.1 K.Hoang 21/09/2021 Fix warnings in PWM examples
|
||||
1.4.0 K.Hoang 22/01/2022 Fix `multiple-definitions` linker error. Fix bug
|
||||
*****************************************************************************************************************************/
|
||||
|
||||
// Modified from stm32 core v2.0.0
|
||||
/*
|
||||
*******************************************************************************
|
||||
Copyright (c) 2019, STMicroelectronics
|
||||
All rights reserved.
|
||||
|
||||
This software component is licensed by ST under BSD 3-Clause license,
|
||||
the "License"; You may not use this file except in compliance with the
|
||||
License. You may obtain a copy of the License at:
|
||||
opensource.org/licenses/BSD-3-Clause
|
||||
|
||||
*******************************************************************************
|
||||
*/
|
||||
#if defined(ARDUINO_GIGA)
|
||||
#include "Gigatimer.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#if defined(HAL_TIM_MODULE_ENABLED) && !defined(HAL_TIM_MODULE_ONLY)
|
||||
|
||||
/* Private Functions */
|
||||
/* Aim of the function is to get _timerObj pointer using htim pointer */
|
||||
/* Highly inspired from magical linux kernel's "container_of" */
|
||||
/* (which was not directly used since not compatible with IAR toolchain) */
|
||||
timerObj_t *get_timer_obj(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
timerObj_t *obj;
|
||||
obj = (timerObj_t *)((char *)htim - offsetof(timerObj_t, handle));
|
||||
return (obj);
|
||||
}
|
||||
|
||||
/**
|
||||
@brief TIMER Initialization - clock init and nvic init
|
||||
@param htim_base: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim_base)
|
||||
{
|
||||
timerObj_t *obj = get_timer_obj(htim_base);
|
||||
enableTimerClock(htim_base);
|
||||
|
||||
// configure Update interrupt
|
||||
HAL_NVIC_SetPriority(getTimerUpIrq(htim_base->Instance), obj->preemptPriority, obj->subPriority);
|
||||
HAL_NVIC_EnableIRQ(getTimerUpIrq(htim_base->Instance));
|
||||
|
||||
if (getTimerCCIrq(htim_base->Instance) != getTimerUpIrq(htim_base->Instance))
|
||||
{
|
||||
// configure Capture Compare interrupt
|
||||
HAL_NVIC_SetPriority(getTimerCCIrq(htim_base->Instance), obj->preemptPriority, obj->subPriority);
|
||||
HAL_NVIC_EnableIRQ(getTimerCCIrq(htim_base->Instance));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@brief TIMER Deinitialization - clock and nvic
|
||||
@param htim_base: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef *htim_base)
|
||||
{
|
||||
disableTimerClock(htim_base);
|
||||
HAL_NVIC_DisableIRQ(getTimerUpIrq(htim_base->Instance));
|
||||
HAL_NVIC_DisableIRQ(getTimerCCIrq(htim_base->Instance));
|
||||
}
|
||||
|
||||
/**
|
||||
@brief Initializes the TIM Output Compare MSP.
|
||||
@param htim: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void HAL_TIM_OC_MspInit(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
timerObj_t *obj = get_timer_obj(htim);
|
||||
enableTimerClock(htim);
|
||||
|
||||
// configure Update interrupt
|
||||
HAL_NVIC_SetPriority(getTimerUpIrq(htim->Instance), obj->preemptPriority, obj->subPriority);
|
||||
HAL_NVIC_EnableIRQ(getTimerUpIrq(htim->Instance));
|
||||
|
||||
if (getTimerCCIrq(htim->Instance) != getTimerUpIrq(htim->Instance))
|
||||
{
|
||||
// configure Capture Compare interrupt
|
||||
HAL_NVIC_SetPriority(getTimerCCIrq(htim->Instance), obj->preemptPriority, obj->subPriority);
|
||||
HAL_NVIC_EnableIRQ(getTimerCCIrq(htim->Instance));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@brief DeInitialize TIM Output Compare MSP.
|
||||
@param htim: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void HAL_TIM_OC_MspDeInit(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
disableTimerClock(htim);
|
||||
HAL_NVIC_DisableIRQ(getTimerUpIrq(htim->Instance));
|
||||
HAL_NVIC_DisableIRQ(getTimerCCIrq(htim->Instance));
|
||||
}
|
||||
|
||||
/**
|
||||
@brief Initializes the TIM Input Capture MSP.
|
||||
@param htim: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void HAL_TIM_IC_MspInit(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
enableTimerClock(htim);
|
||||
}
|
||||
|
||||
/**
|
||||
@brief DeInitialize TIM Input Capture MSP.
|
||||
@param htim: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void HAL_TIM_IC_MspDeInit(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
disableTimerClock(htim);
|
||||
}
|
||||
|
||||
/* Exported functions */
|
||||
/**
|
||||
@brief Enable the timer clock
|
||||
@param htim: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void enableTimerClock(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
// Enable TIM clock
|
||||
#if defined(TIM1_BASE)
|
||||
if (htim->Instance == TIM1)
|
||||
{
|
||||
__HAL_RCC_TIM1_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM2_BASE)
|
||||
|
||||
if (htim->Instance == TIM2)
|
||||
{
|
||||
__HAL_RCC_TIM2_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM3_BASE)
|
||||
|
||||
if (htim->Instance == TIM3)
|
||||
{
|
||||
__HAL_RCC_TIM3_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM4_BASE)
|
||||
|
||||
if (htim->Instance == TIM4)
|
||||
{
|
||||
__HAL_RCC_TIM4_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM5_BASE)
|
||||
|
||||
if (htim->Instance == TIM5)
|
||||
{
|
||||
__HAL_RCC_TIM5_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM6_BASE)
|
||||
|
||||
if (htim->Instance == TIM6)
|
||||
{
|
||||
__HAL_RCC_TIM6_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM7_BASE)
|
||||
|
||||
if (htim->Instance == TIM7)
|
||||
{
|
||||
__HAL_RCC_TIM7_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM8_BASE)
|
||||
|
||||
if (htim->Instance == TIM8)
|
||||
{
|
||||
__HAL_RCC_TIM8_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM9_BASE)
|
||||
|
||||
if (htim->Instance == TIM9)
|
||||
{
|
||||
__HAL_RCC_TIM9_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM10_BASE)
|
||||
|
||||
if (htim->Instance == TIM10)
|
||||
{
|
||||
__HAL_RCC_TIM10_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM11_BASE)
|
||||
|
||||
if (htim->Instance == TIM11)
|
||||
{
|
||||
__HAL_RCC_TIM11_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM12_BASE)
|
||||
|
||||
if (htim->Instance == TIM12)
|
||||
{
|
||||
__HAL_RCC_TIM12_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM13_BASE)
|
||||
|
||||
if (htim->Instance == TIM13)
|
||||
{
|
||||
__HAL_RCC_TIM13_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM14_BASE)
|
||||
|
||||
if (htim->Instance == TIM14)
|
||||
{
|
||||
__HAL_RCC_TIM14_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM15_BASE)
|
||||
|
||||
if (htim->Instance == TIM15)
|
||||
{
|
||||
__HAL_RCC_TIM15_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM16_BASE)
|
||||
|
||||
if (htim->Instance == TIM16)
|
||||
{
|
||||
__HAL_RCC_TIM16_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM17_BASE)
|
||||
|
||||
if (htim->Instance == TIM17)
|
||||
{
|
||||
__HAL_RCC_TIM17_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM18_BASE)
|
||||
|
||||
if (htim->Instance == TIM18)
|
||||
{
|
||||
__HAL_RCC_TIM18_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM19_BASE)
|
||||
|
||||
if (htim->Instance == TIM19)
|
||||
{
|
||||
__HAL_RCC_TIM19_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM20_BASE)
|
||||
|
||||
if (htim->Instance == TIM20)
|
||||
{
|
||||
__HAL_RCC_TIM20_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM21_BASE)
|
||||
|
||||
if (htim->Instance == TIM21)
|
||||
{
|
||||
__HAL_RCC_TIM21_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM22_BASE)
|
||||
|
||||
if (htim->Instance == TIM22)
|
||||
{
|
||||
__HAL_RCC_TIM22_CLK_ENABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
@brief Disable the timer clock
|
||||
@param htim: TIM handle
|
||||
@retval None
|
||||
*/
|
||||
void disableTimerClock(TIM_HandleTypeDef *htim)
|
||||
{
|
||||
// Enable TIM clock
|
||||
#if defined(TIM1_BASE)
|
||||
if (htim->Instance == TIM1)
|
||||
{
|
||||
__HAL_RCC_TIM1_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM2_BASE)
|
||||
|
||||
if (htim->Instance == TIM2)
|
||||
{
|
||||
__HAL_RCC_TIM2_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM3_BASE)
|
||||
|
||||
if (htim->Instance == TIM3)
|
||||
{
|
||||
__HAL_RCC_TIM3_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM4_BASE)
|
||||
|
||||
if (htim->Instance == TIM4)
|
||||
{
|
||||
__HAL_RCC_TIM4_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM5_BASE)
|
||||
|
||||
if (htim->Instance == TIM5)
|
||||
{
|
||||
__HAL_RCC_TIM5_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM6_BASE)
|
||||
|
||||
if (htim->Instance == TIM6)
|
||||
{
|
||||
__HAL_RCC_TIM6_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM7_BASE)
|
||||
|
||||
if (htim->Instance == TIM7)
|
||||
{
|
||||
__HAL_RCC_TIM7_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM8_BASE)
|
||||
|
||||
if (htim->Instance == TIM8)
|
||||
{
|
||||
__HAL_RCC_TIM8_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM9_BASE)
|
||||
|
||||
if (htim->Instance == TIM9)
|
||||
{
|
||||
__HAL_RCC_TIM9_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM10_BASE)
|
||||
|
||||
if (htim->Instance == TIM10)
|
||||
{
|
||||
__HAL_RCC_TIM10_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM11_BASE)
|
||||
|
||||
if (htim->Instance == TIM11)
|
||||
{
|
||||
__HAL_RCC_TIM11_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM12_BASE)
|
||||
|
||||
if (htim->Instance == TIM12)
|
||||
{
|
||||
__HAL_RCC_TIM12_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM13_BASE)
|
||||
|
||||
if (htim->Instance == TIM13)
|
||||
{
|
||||
__HAL_RCC_TIM13_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM14_BASE)
|
||||
|
||||
if (htim->Instance == TIM14)
|
||||
{
|
||||
__HAL_RCC_TIM14_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM15_BASE)
|
||||
|
||||
if (htim->Instance == TIM15)
|
||||
{
|
||||
__HAL_RCC_TIM15_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM16_BASE)
|
||||
|
||||
if (htim->Instance == TIM16)
|
||||
{
|
||||
__HAL_RCC_TIM16_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM17_BASE)
|
||||
|
||||
if (htim->Instance == TIM17)
|
||||
{
|
||||
__HAL_RCC_TIM17_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM18_BASE)
|
||||
|
||||
if (htim->Instance == TIM18)
|
||||
{
|
||||
__HAL_RCC_TIM18_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM19_BASE)
|
||||
|
||||
if (htim->Instance == TIM19)
|
||||
{
|
||||
__HAL_RCC_TIM19_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM20_BASE)
|
||||
|
||||
if (htim->Instance == TIM20)
|
||||
{
|
||||
__HAL_RCC_TIM20_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM21_BASE)
|
||||
|
||||
if (htim->Instance == TIM21)
|
||||
{
|
||||
__HAL_RCC_TIM21_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
#if defined(TIM22_BASE)
|
||||
|
||||
if (htim->Instance == TIM22)
|
||||
{
|
||||
__HAL_RCC_TIM22_CLK_DISABLE();
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
@brief This function return IRQ number corresponding to update interrupt event of timer instance.
|
||||
@param tim: timer instance
|
||||
@retval IRQ number
|
||||
*/
|
||||
IRQn_Type getTimerUpIrq(TIM_TypeDef *tim)
|
||||
{
|
||||
IRQn_Type IRQn = NonMaskableInt_IRQn;
|
||||
|
||||
if (tim != (TIM_TypeDef *)NC)
|
||||
{
|
||||
/* Get IRQn depending on TIM instance */
|
||||
switch ((uint32_t)tim)
|
||||
{
|
||||
#if defined(TIM1_BASE)
|
||||
|
||||
case (uint32_t)TIM1_BASE:
|
||||
IRQn = TIM1_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM2_BASE)
|
||||
|
||||
case (uint32_t)TIM2_BASE:
|
||||
IRQn = TIM2_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM3_BASE)
|
||||
|
||||
case (uint32_t)TIM3_BASE:
|
||||
IRQn = TIM3_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM4_BASE)
|
||||
|
||||
case (uint32_t)TIM4_BASE:
|
||||
IRQn = TIM4_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM5_BASE)
|
||||
|
||||
case (uint32_t)TIM5_BASE:
|
||||
IRQn = TIM5_IRQn;
|
||||
break;
|
||||
#endif
|
||||
|
||||
// KH
|
||||
#if 0
|
||||
#if defined(TIM6_BASE)
|
||||
|
||||
case (uint32_t)TIM6_BASE:
|
||||
IRQn = TIM6_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#endif
|
||||
//////
|
||||
|
||||
#if defined(TIM7_BASE)
|
||||
|
||||
case (uint32_t)TIM7_BASE:
|
||||
IRQn = TIM7_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM8_BASE)
|
||||
|
||||
case (uint32_t)TIM8_BASE:
|
||||
IRQn = TIM8_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM9_BASE)
|
||||
|
||||
case (uint32_t)TIM9_BASE:
|
||||
IRQn = TIM9_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM10_BASE)
|
||||
|
||||
case (uint32_t)TIM10_BASE:
|
||||
IRQn = TIM10_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM11_BASE)
|
||||
|
||||
case (uint32_t)TIM11_BASE:
|
||||
IRQn = TIM11_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM12_BASE)
|
||||
|
||||
case (uint32_t)TIM12_BASE:
|
||||
IRQn = TIM12_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM13_BASE)
|
||||
|
||||
case (uint32_t)TIM13_BASE:
|
||||
IRQn = TIM13_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM14_BASE)
|
||||
|
||||
case (uint32_t)TIM14_BASE:
|
||||
IRQn = TIM14_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM15_BASE)
|
||||
|
||||
case (uint32_t)TIM15_BASE:
|
||||
IRQn = TIM15_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM16_BASE)
|
||||
|
||||
case (uint32_t)TIM16_BASE:
|
||||
IRQn = TIM16_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM17_BASE)
|
||||
|
||||
case (uint32_t)TIM17_BASE:
|
||||
IRQn = TIM17_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM18_BASE)
|
||||
|
||||
case (uint32_t)TIM18_BASE:
|
||||
IRQn = TIM18_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM19_BASE)
|
||||
|
||||
case (uint32_t)TIM19_BASE:
|
||||
IRQn = TIM19_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM20_BASE)
|
||||
|
||||
case (uint32_t)TIM20_BASE:
|
||||
IRQn = TIM20_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM21_BASE)
|
||||
|
||||
case (uint32_t)TIM21_BASE:
|
||||
IRQn = TIM21_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM22_BASE)
|
||||
|
||||
case (uint32_t)TIM22_BASE:
|
||||
IRQn = TIM22_IRQn;
|
||||
break;
|
||||
#endif
|
||||
|
||||
default:
|
||||
//_Error_Handler("TIM: Unknown timer IRQn", (int)tim);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return IRQn;
|
||||
}
|
||||
|
||||
/**
|
||||
@brief This function return IRQ number corresponding to Capture or Compare interrupt event of timer instance.
|
||||
@param tim: timer instance
|
||||
@retval IRQ number
|
||||
*/
|
||||
IRQn_Type getTimerCCIrq(TIM_TypeDef *tim)
|
||||
{
|
||||
IRQn_Type IRQn = NonMaskableInt_IRQn;
|
||||
|
||||
if (tim != (TIM_TypeDef *)NC)
|
||||
{
|
||||
/* Get IRQn depending on TIM instance */
|
||||
switch ((uint32_t)tim)
|
||||
{
|
||||
#if defined(TIM1_BASE)
|
||||
|
||||
case (uint32_t)TIM1_BASE:
|
||||
IRQn = TIM1_CC_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM2_BASE)
|
||||
|
||||
case (uint32_t)TIM2_BASE:
|
||||
IRQn = TIM2_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM3_BASE)
|
||||
|
||||
case (uint32_t)TIM3_BASE:
|
||||
IRQn = TIM3_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM4_BASE)
|
||||
|
||||
case (uint32_t)TIM4_BASE:
|
||||
IRQn = TIM4_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM5_BASE)
|
||||
|
||||
case (uint32_t)TIM5_BASE:
|
||||
IRQn = TIM5_IRQn;
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
// KH
|
||||
#if defined(TIM6_BASE)
|
||||
|
||||
case (uint32_t)TIM6_BASE:
|
||||
IRQn = TIM6_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#endif
|
||||
//////
|
||||
|
||||
#if defined(TIM7_BASE)
|
||||
|
||||
case (uint32_t)TIM7_BASE:
|
||||
IRQn = TIM7_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM8_BASE)
|
||||
|
||||
case (uint32_t)TIM8_BASE:
|
||||
IRQn = TIM8_CC_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM9_BASE)
|
||||
|
||||
case (uint32_t)TIM9_BASE:
|
||||
IRQn = TIM9_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM10_BASE)
|
||||
|
||||
case (uint32_t)TIM10_BASE:
|
||||
IRQn = TIM10_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM11_BASE)
|
||||
|
||||
case (uint32_t)TIM11_BASE:
|
||||
IRQn = TIM11_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM12_BASE)
|
||||
|
||||
case (uint32_t)TIM12_BASE:
|
||||
IRQn = TIM12_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM13_BASE)
|
||||
|
||||
case (uint32_t)TIM13_BASE:
|
||||
IRQn = TIM13_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM14_BASE)
|
||||
|
||||
case (uint32_t)TIM14_BASE:
|
||||
IRQn = TIM14_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM15_BASE)
|
||||
|
||||
case (uint32_t)TIM15_BASE:
|
||||
IRQn = TIM15_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM16_BASE)
|
||||
|
||||
case (uint32_t)TIM16_BASE:
|
||||
IRQn = TIM16_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM17_BASE)
|
||||
|
||||
case (uint32_t)TIM17_BASE:
|
||||
IRQn = TIM17_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM18_BASE)
|
||||
|
||||
case (uint32_t)TIM18_BASE:
|
||||
IRQn = TIM18_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM19_BASE)
|
||||
|
||||
case (uint32_t)TIM19_BASE:
|
||||
IRQn = TIM19_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM20_BASE)
|
||||
|
||||
case (uint32_t)TIM20_BASE:
|
||||
IRQn = TIM20_CC_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM21_BASE)
|
||||
|
||||
case (uint32_t)TIM21_BASE:
|
||||
IRQn = TIM21_IRQn;
|
||||
break;
|
||||
#endif
|
||||
#if defined(TIM22_BASE)
|
||||
|
||||
case (uint32_t)TIM22_BASE:
|
||||
IRQn = TIM22_IRQn;
|
||||
break;
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
//_Error_Handler("TIM: Unknown timer IRQn", (int)tim);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return IRQn;
|
||||
}
|
||||
|
||||
/**
|
||||
@brief This function return the timer clock source.
|
||||
@param tim: timer instance
|
||||
@retval 1 = PCLK1 or 2 = PCLK2
|
||||
*/
|
||||
uint8_t getTimerClkSrc(TIM_TypeDef *tim)
|
||||
{
|
||||
uint8_t clkSrc = 0;
|
||||
|
||||
if (tim != (TIM_TypeDef *)NC)
|
||||
#if defined(STM32F0xx) || defined(STM32G0xx)
|
||||
/* TIMx source CLK is PCKL1 */
|
||||
clkSrc = 1;
|
||||
|
||||
#else
|
||||
{
|
||||
/* Get source clock depending on TIM instance */
|
||||
switch ((uint32_t)tim)
|
||||
{
|
||||
#if defined(TIM2_BASE)
|
||||
|
||||
case (uint32_t)TIM2:
|
||||
#endif
|
||||
#if defined(TIM3_BASE)
|
||||
case (uint32_t)TIM3:
|
||||
#endif
|
||||
#if defined(TIM4_BASE)
|
||||
case (uint32_t)TIM4:
|
||||
#endif
|
||||
#if defined(TIM5_BASE)
|
||||
case (uint32_t)TIM5:
|
||||
#endif
|
||||
#if defined(TIM6_BASE)
|
||||
case (uint32_t)TIM6:
|
||||
#endif
|
||||
#if defined(TIM7_BASE)
|
||||
case (uint32_t)TIM7:
|
||||
#endif
|
||||
#if defined(TIM12_BASE)
|
||||
case (uint32_t)TIM12:
|
||||
#endif
|
||||
#if defined(TIM13_BASE)
|
||||
case (uint32_t)TIM13:
|
||||
#endif
|
||||
#if defined(TIM14_BASE)
|
||||
case (uint32_t)TIM14:
|
||||
#endif
|
||||
#if defined(TIM18_BASE)
|
||||
case (uint32_t)TIM18:
|
||||
#endif
|
||||
clkSrc = 1;
|
||||
break;
|
||||
#if defined(TIM1_BASE)
|
||||
|
||||
case (uint32_t)TIM1:
|
||||
#endif
|
||||
#if defined(TIM8_BASE)
|
||||
case (uint32_t)TIM8:
|
||||
#endif
|
||||
#if defined(TIM9_BASE)
|
||||
case (uint32_t)TIM9:
|
||||
#endif
|
||||
#if defined(TIM10_BASE)
|
||||
case (uint32_t)TIM10:
|
||||
#endif
|
||||
#if defined(TIM11_BASE)
|
||||
case (uint32_t)TIM11:
|
||||
#endif
|
||||
#if defined(TIM15_BASE)
|
||||
case (uint32_t)TIM15:
|
||||
#endif
|
||||
#if defined(TIM16_BASE)
|
||||
case (uint32_t)TIM16:
|
||||
#endif
|
||||
#if defined(TIM17_BASE)
|
||||
case (uint32_t)TIM17:
|
||||
#endif
|
||||
#if defined(TIM19_BASE)
|
||||
case (uint32_t)TIM19:
|
||||
#endif
|
||||
#if defined(TIM20_BASE)
|
||||
case (uint32_t)TIM20:
|
||||
#endif
|
||||
#if defined(TIM21_BASE)
|
||||
case (uint32_t)TIM21:
|
||||
#endif
|
||||
#if defined(TIM22_BASE)
|
||||
case (uint32_t)TIM22:
|
||||
#endif
|
||||
clkSrc = 2;
|
||||
break;
|
||||
|
||||
default:
|
||||
////_Error_Handler("TIM: Unknown timer instance", (int)tim);
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return clkSrc;
|
||||
}
|
||||
|
||||
|
||||
#endif /* HAL_TIM_MODULE_ENABLED && !HAL_TIM_MODULE_ONLY */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
198
Gigatimer.h
Normal file
198
Gigatimer.h
Normal file
@@ -0,0 +1,198 @@
|
||||
/****************************************************************************************************************************
|
||||
timer.h
|
||||
|
||||
For Portenta_H7 boards
|
||||
Written by Khoi Hoang
|
||||
|
||||
Built by Khoi Hoang https://github.com/khoih-prog/Portenta_H7_TimerInterrupt
|
||||
Licensed under MIT license
|
||||
|
||||
Now even you use all these new 16 ISR-based timers,with their maximum interval practically unlimited (limited only by
|
||||
unsigned long miliseconds), you just consume only one Portenta_H7 STM32 timer and avoid conflicting with other cores' tasks.
|
||||
The accuracy is nearly perfect compared to software timers. The most important feature is they're ISR-based timers
|
||||
Therefore, their executions are not blocked by bad-behaving functions / tasks.
|
||||
This important feature is absolutely necessary for mission-critical tasks.
|
||||
|
||||
Version: 1.4.0
|
||||
|
||||
Version Modified By Date Comments
|
||||
------- ----------- ---------- -----------
|
||||
1.2.1 K.Hoang 15/09/2021 Initial coding for Portenta_H7
|
||||
1.3.0 K.Hoang 17/09/2021 Add PWM features and examples
|
||||
1.3.1 K.Hoang 21/09/2021 Fix warnings in PWM examples
|
||||
1.4.0 K.Hoang 22/01/2022 Fix `multiple-definitions` linker error. Fix bug
|
||||
*****************************************************************************************************************************/
|
||||
|
||||
// Modified from stm32 core v2.0.0
|
||||
|
||||
/*
|
||||
*******************************************************************************
|
||||
Copyright (c) 2019, STMicroelectronics
|
||||
All rights reserved.
|
||||
|
||||
This software component is licensed by ST under BSD 3-Clause license,
|
||||
the "License"; You may not use this file except in compliance with the
|
||||
License. You may obtain a copy of the License at:
|
||||
opensource.org/licenses/BSD-3-Clause
|
||||
|
||||
*******************************************************************************
|
||||
*/
|
||||
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __GIGATIMER_H
|
||||
#define __GIGATIMER_H
|
||||
#if defined(ARDUINO_GIGA)
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "PinNames.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#if defined(HAL_TIM_MODULE_ENABLED) && !defined(HAL_TIM_MODULE_ONLY)
|
||||
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
#ifndef TIM_IRQ_PRIO
|
||||
#if (__CORTEX_M == 0x00U)
|
||||
#define TIM_IRQ_PRIO 3
|
||||
#else
|
||||
#define TIM_IRQ_PRIO 14
|
||||
#endif /* __CORTEX_M */
|
||||
|
||||
#endif /* TIM_IRQ_PRIO */
|
||||
|
||||
#ifndef TIM_IRQ_SUBPRIO
|
||||
#define TIM_IRQ_SUBPRIO 0
|
||||
#endif
|
||||
|
||||
#if defined(TIM1_BASE) && !defined(TIM1_IRQn)
|
||||
#define TIM1_IRQn TIM1_UP_IRQn
|
||||
#define TIM1_IRQHandler TIM1_UP_IRQHandler
|
||||
#endif
|
||||
|
||||
#if defined(TIM8_BASE) && !defined(TIM8_IRQn)
|
||||
#define TIM8_IRQn TIM8_UP_TIM13_IRQn
|
||||
#define TIM8_IRQHandler TIM8_UP_TIM13_IRQHandler
|
||||
#endif
|
||||
|
||||
#if defined(TIM12_BASE) && !defined(TIM12_IRQn)
|
||||
#define TIM12_IRQn TIM8_BRK_TIM12_IRQn
|
||||
#define TIM12_IRQHandler TIM8_BRK_TIM12_IRQHandler
|
||||
#endif
|
||||
|
||||
#if defined(TIM13_BASE) && !defined(TIM13_IRQn)
|
||||
#define TIM13_IRQn TIM8_UP_TIM13_IRQn
|
||||
#endif
|
||||
|
||||
#if defined(TIM14_BASE) && !defined(TIM14_IRQn)
|
||||
#define TIM14_IRQn TIM8_TRG_COM_TIM14_IRQn
|
||||
#define TIM14_IRQHandler TIM8_TRG_COM_TIM14_IRQHandler
|
||||
#endif
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
#if defined(TIM1_BASE)
|
||||
TIMER1_INDEX,
|
||||
#endif
|
||||
#if defined(TIM2_BASE)
|
||||
TIMER2_INDEX,
|
||||
#endif
|
||||
#if defined(TIM3_BASE)
|
||||
TIMER3_INDEX,
|
||||
#endif
|
||||
#if defined(TIM4_BASE)
|
||||
TIMER4_INDEX,
|
||||
#endif
|
||||
#if defined(TIM5_BASE)
|
||||
TIMER5_INDEX,
|
||||
#endif
|
||||
#if defined(TIM6_BASE)
|
||||
TIMER6_INDEX,
|
||||
#endif
|
||||
#if defined(TIM7_BASE)
|
||||
TIMER7_INDEX,
|
||||
#endif
|
||||
#if defined(TIM8_BASE)
|
||||
TIMER8_INDEX,
|
||||
#endif
|
||||
#if defined(TIM9_BASE)
|
||||
TIMER9_INDEX,
|
||||
#endif
|
||||
#if defined(TIM10_BASE)
|
||||
TIMER10_INDEX,
|
||||
#endif
|
||||
#if defined(TIM11_BASE)
|
||||
TIMER11_INDEX,
|
||||
#endif
|
||||
#if defined(TIM12_BASE)
|
||||
TIMER12_INDEX,
|
||||
#endif
|
||||
#if defined(TIM13_BASE)
|
||||
TIMER13_INDEX,
|
||||
#endif
|
||||
#if defined(TIM14_BASE)
|
||||
TIMER14_INDEX,
|
||||
#endif
|
||||
#if defined(TIM15_BASE)
|
||||
TIMER15_INDEX,
|
||||
#endif
|
||||
#if defined(TIM16_BASE)
|
||||
TIMER16_INDEX,
|
||||
#endif
|
||||
#if defined(TIM17_BASE)
|
||||
TIMER17_INDEX,
|
||||
#endif
|
||||
#if defined(TIM18_BASE)
|
||||
TIMER18_INDEX,
|
||||
#endif
|
||||
#if defined(TIM19_BASE)
|
||||
TIMER19_INDEX,
|
||||
#endif
|
||||
#if defined(TIM20_BASE)
|
||||
TIMER20_INDEX,
|
||||
#endif
|
||||
#if defined(TIM21_BASE)
|
||||
TIMER21_INDEX,
|
||||
#endif
|
||||
#if defined(TIM22_BASE)
|
||||
TIMER22_INDEX,
|
||||
#endif
|
||||
|
||||
TIMER_NUM,
|
||||
UNKNOWN_TIMER = 0XFFFF
|
||||
} timer_index_t;
|
||||
|
||||
|
||||
// This structure is used to be able to get HardwareTimer instance (C++ class)
|
||||
// from handler (C structure) specially for interrupt management
|
||||
typedef struct
|
||||
{
|
||||
// Those 2 first fields must remain in this order at the beginning of the structure
|
||||
void *__this;
|
||||
TIM_HandleTypeDef handle;
|
||||
uint32_t preemptPriority;
|
||||
uint32_t subPriority;
|
||||
} timerObj_t;
|
||||
|
||||
/* Exported functions ------------------------------------------------------- */
|
||||
timerObj_t *get_timer_obj(TIM_HandleTypeDef *htim);
|
||||
|
||||
void enableTimerClock(TIM_HandleTypeDef *htim);
|
||||
void disableTimerClock(TIM_HandleTypeDef *htim);
|
||||
|
||||
uint32_t getTimerIrq(TIM_TypeDef *tim);
|
||||
uint8_t getTimerClkSrc(TIM_TypeDef *tim);
|
||||
|
||||
IRQn_Type getTimerUpIrq(TIM_TypeDef *tim);
|
||||
IRQn_Type getTimerCCIrq(TIM_TypeDef *tim);
|
||||
|
||||
#endif /* HAL_TIM_MODULE_ENABLED && !HAL_TIM_MODULE_ONLY */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
#endif /* __GIGATIMER_H */
|
||||
|
||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
@@ -110,6 +110,7 @@ void I2CManagerClass::I2C_setClock(uint32_t i2cClockSpeed) {
|
||||
// Calculate a rise time appropriate to the requested bus speed
|
||||
// Use 10x the rise time spec to enable integer divide of 50ns clock period
|
||||
uint16_t t_rise;
|
||||
uint32_t ccr_freq;
|
||||
|
||||
while (s->CR1 & I2C_CR1_STOP); // Prevents lockup by guarding further
|
||||
// writes to CR1 while STOP is being executed!
|
||||
|
@@ -35,13 +35,21 @@
|
||||
#define WIRE_HAS_TIMEOUT
|
||||
#endif
|
||||
|
||||
#if defined(GIGA_I2C_1)
|
||||
#define DCCEX_WIRE Wire1
|
||||
#else
|
||||
#define DCCEX_WIRE Wire
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/***************************************************************************
|
||||
* Initialise I2C interface software
|
||||
***************************************************************************/
|
||||
void I2CManagerClass::_initialise() {
|
||||
Wire.begin();
|
||||
DCCEX_WIRE.begin();
|
||||
#if defined(WIRE_HAS_TIMEOUT)
|
||||
Wire.setWireTimeout(_timeout, true);
|
||||
DCCEX_WIRE.setWireTimeout(_timeout, true);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -50,7 +58,7 @@ void I2CManagerClass::_initialise() {
|
||||
* on Arduino. Mega4809 supports 1000000 (Fast+) too.
|
||||
***************************************************************************/
|
||||
void I2CManagerClass::_setClock(unsigned long i2cClockSpeed) {
|
||||
Wire.setClock(i2cClockSpeed);
|
||||
DCCEX_WIRE.setClock(i2cClockSpeed);
|
||||
}
|
||||
|
||||
/***************************************************************************
|
||||
@@ -61,7 +69,7 @@ void I2CManagerClass::_setClock(unsigned long i2cClockSpeed) {
|
||||
void I2CManagerClass::setTimeout(unsigned long value) {
|
||||
_timeout = value;
|
||||
#if defined(WIRE_HAS_TIMEOUT)
|
||||
Wire.setWireTimeout(value, true);
|
||||
DCCEX_WIRE.setWireTimeout(value, true);
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -74,7 +82,7 @@ static uint8_t muxSelect(I2CAddress address) {
|
||||
I2CMux muxNo = address.muxNumber();
|
||||
I2CSubBus subBus = address.subBus();
|
||||
if (muxNo != I2CMux_None) {
|
||||
Wire.beginTransmission(I2C_MUX_BASE_ADDRESS+muxNo);
|
||||
DCCEX_WIRE.beginTransmission(I2C_MUX_BASE_ADDRESS+muxNo);
|
||||
uint8_t data = (subBus == SubBus_All) ? 0xff :
|
||||
(subBus == SubBus_None) ? 0x00 :
|
||||
#if defined(I2CMUX_PCA9547)
|
||||
@@ -86,8 +94,8 @@ static uint8_t muxSelect(I2CAddress address) {
|
||||
// with a bit set for the subBus to be enabled
|
||||
1 << subBus;
|
||||
#endif
|
||||
Wire.write(&data, 1);
|
||||
return Wire.endTransmission(true); // have to release I2C bus for it to work
|
||||
DCCEX_WIRE.write(&data, 1);
|
||||
return DCCEX_WIRE.endTransmission(true); // have to release I2C bus for it to work
|
||||
}
|
||||
return I2C_STATUS_OK;
|
||||
}
|
||||
@@ -110,9 +118,9 @@ uint8_t I2CManagerClass::write(I2CAddress address, const uint8_t buffer[], uint8
|
||||
#endif
|
||||
// Only send new transaction if address is non-zero.
|
||||
if (muxStatus == I2C_STATUS_OK && address != 0) {
|
||||
Wire.beginTransmission(address);
|
||||
if (size > 0) Wire.write(buffer, size);
|
||||
status = Wire.endTransmission();
|
||||
DCCEX_WIRE.beginTransmission(address);
|
||||
if (size > 0) DCCEX_WIRE.write(buffer, size);
|
||||
status = DCCEX_WIRE.endTransmission();
|
||||
}
|
||||
#ifdef I2C_EXTENDED_ADDRESS
|
||||
// Deselect MUX if there's more than one MUX present, to avoid having multiple ones selected
|
||||
@@ -161,25 +169,25 @@ uint8_t I2CManagerClass::read(I2CAddress address, uint8_t readBuffer[], uint8_t
|
||||
// Only start new transaction if address is non-zero.
|
||||
if (muxStatus == I2C_STATUS_OK && address != 0) {
|
||||
if (writeSize > 0) {
|
||||
Wire.beginTransmission(address);
|
||||
Wire.write(writeBuffer, writeSize);
|
||||
status = Wire.endTransmission(false); // Don't free bus yet
|
||||
DCCEX_WIRE.beginTransmission(address);
|
||||
DCCEX_WIRE.write(writeBuffer, writeSize);
|
||||
status = DCCEX_WIRE.endTransmission(false); // Don't free bus yet
|
||||
}
|
||||
if (status == I2C_STATUS_OK) {
|
||||
#ifdef WIRE_HAS_TIMEOUT
|
||||
Wire.clearWireTimeoutFlag();
|
||||
Wire.requestFrom(address, (size_t)readSize);
|
||||
if (!Wire.getWireTimeoutFlag()) {
|
||||
while (Wire.available() && nBytes < readSize)
|
||||
readBuffer[nBytes++] = Wire.read();
|
||||
DCCEX_WIRE.clearWireTimeoutFlag();
|
||||
DCCEX_WIRE.requestFrom(address, (size_t)readSize);
|
||||
if (!DCCEX_WIRE.getWireTimeoutFlag()) {
|
||||
while (DCCEX_WIRE.available() && nBytes < readSize)
|
||||
readBuffer[nBytes++] = DCCEX_WIRE.read();
|
||||
if (nBytes < readSize) status = I2C_STATUS_TRUNCATED;
|
||||
} else {
|
||||
status = I2C_STATUS_TIMEOUT;
|
||||
}
|
||||
#else
|
||||
Wire.requestFrom(address, (size_t)readSize);
|
||||
while (Wire.available() && nBytes < readSize)
|
||||
readBuffer[nBytes++] = Wire.read();
|
||||
DCCEX_WIRE.requestFrom(address, (size_t)readSize);
|
||||
while (DCCEX_WIRE.available() && nBytes < readSize)
|
||||
readBuffer[nBytes++] = DCCEX_WIRE.read();
|
||||
if (nBytes < readSize) status = I2C_STATUS_TRUNCATED;
|
||||
#endif
|
||||
}
|
||||
|
44
IODevice.cpp
44
IODevice.cpp
@@ -33,7 +33,7 @@
|
||||
|
||||
// Link to halSetup function. If not defined, the function reference will be NULL.
|
||||
extern __attribute__((weak)) void halSetup();
|
||||
extern __attribute__((weak)) bool exrailHalSetup();
|
||||
extern __attribute__((weak)) void exrailHalSetup();
|
||||
|
||||
//==================================================================================================================
|
||||
// Static methods
|
||||
@@ -60,31 +60,34 @@ void IODevice::begin() {
|
||||
halSetup();
|
||||
|
||||
// include any HAL devices defined in exrail.
|
||||
bool ignoreDefaults=false;
|
||||
if (exrailHalSetup)
|
||||
ignoreDefaults=exrailHalSetup();
|
||||
if (ignoreDefaults) return;
|
||||
|
||||
exrailHalSetup();
|
||||
|
||||
// Predefine two PCA9685 modules 0x40-0x41 if no conflicts
|
||||
// Allocates 32 pins 100-131
|
||||
const bool silent=true; // no message if these conflict
|
||||
if (checkNoOverlap(100, 16, 0x40, silent)) {
|
||||
if (checkNoOverlap(100, 16, 0x40)) {
|
||||
PCA9685::create(100, 16, 0x40);
|
||||
}
|
||||
|
||||
if (checkNoOverlap(116, 16, 0x41, silent)) {
|
||||
} else {
|
||||
DIAG(F("Default PCA9685 at I2C 0x40 disabled due to configured user device"));
|
||||
}
|
||||
if (checkNoOverlap(116, 16, 0x41)) {
|
||||
PCA9685::create(116, 16, 0x41);
|
||||
}
|
||||
} else {
|
||||
DIAG(F("Default PCA9685 at I2C 0x41 disabled due to configured user device"));
|
||||
}
|
||||
|
||||
// Predefine two MCP23017 module 0x20/0x21 if no conflicts
|
||||
// Allocates 32 pins 164-195
|
||||
if (checkNoOverlap(164, 16, 0x20, silent)) {
|
||||
if (checkNoOverlap(164, 16, 0x20)) {
|
||||
MCP23017::create(164, 16, 0x20);
|
||||
}
|
||||
|
||||
if (checkNoOverlap(180, 16, 0x21, silent)) {
|
||||
} else {
|
||||
DIAG(F("Default MCP23017 at I2C 0x20 disabled due to configured user device"));
|
||||
}
|
||||
if (checkNoOverlap(180, 16, 0x21)) {
|
||||
MCP23017::create(180, 16, 0x21);
|
||||
}
|
||||
} else {
|
||||
DIAG(F("Default MCP23017 at I2C 0x21 disabled due to configured user device"));
|
||||
}
|
||||
}
|
||||
|
||||
// reset() function to reinitialise all devices
|
||||
@@ -336,10 +339,7 @@ IODevice *IODevice::findDeviceFollowing(VPIN vpin) {
|
||||
// returns true if pins DONT overlap with existing device
|
||||
// TODO: Move the I2C address reservation and checks into the I2CManager code.
|
||||
// That will enable non-HAL devices to reserve I2C addresses too.
|
||||
// Silent is used by the default setup so that there is no message if the default
|
||||
// device has already been handled by the user setup.
|
||||
bool IODevice::checkNoOverlap(VPIN firstPin, uint8_t nPins,
|
||||
I2CAddress i2cAddress, bool silent) {
|
||||
bool IODevice::checkNoOverlap(VPIN firstPin, uint8_t nPins, I2CAddress i2cAddress) {
|
||||
#ifdef DIAG_IO
|
||||
DIAG(F("Check no overlap %u %u %s"), firstPin,nPins,i2cAddress.toString());
|
||||
#endif
|
||||
@@ -352,14 +352,14 @@ bool IODevice::checkNoOverlap(VPIN firstPin, uint8_t nPins,
|
||||
VPIN lastDevPin=firstDevPin+dev->_nPins-1;
|
||||
bool noOverlap= firstPin>lastDevPin || lastPin<firstDevPin;
|
||||
if (!noOverlap) {
|
||||
if (!silent) DIAG(F("WARNING HAL Overlap, redefinition of Vpins %u to %u ignored."),
|
||||
DIAG(F("WARNING HAL Overlap, redefinition of Vpins %u to %u ignored."),
|
||||
firstPin, lastPin);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// Check for overlapping I2C address
|
||||
if (i2cAddress && dev->_I2CAddress==i2cAddress) {
|
||||
if (!silent) DIAG(F("WARNING HAL Overlap. i2c Addr %s ignored."),i2cAddress.toString());
|
||||
DIAG(F("WARNING HAL Overlap. i2c Addr %s ignored."),i2cAddress.toString());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
@@ -166,8 +166,7 @@ public:
|
||||
void setGPIOInterruptPin(int16_t pinNumber);
|
||||
|
||||
// Method to check if pins will overlap before creating new device.
|
||||
static bool checkNoOverlap(VPIN firstPin, uint8_t nPins=1,
|
||||
I2CAddress i2cAddress=0, bool silent=false);
|
||||
static bool checkNoOverlap(VPIN firstPin, uint8_t nPins=1, I2CAddress i2cAddress=0);
|
||||
|
||||
// Method used by IODevice filters to locate slave pins that may be overlayed by their own
|
||||
// pin range.
|
||||
@@ -543,10 +542,8 @@ protected:
|
||||
#include "IO_MCP23017.h"
|
||||
#include "IO_PCF8574.h"
|
||||
#include "IO_PCF8575.h"
|
||||
#include "IO_PCA9555.h"
|
||||
#include "IO_duinoNodes.h"
|
||||
#include "IO_EXIOExpander.h"
|
||||
#include "IO_trainbrains.h"
|
||||
|
||||
|
||||
#endif // iodevice_h
|
||||
|
@@ -1,6 +1,5 @@
|
||||
/*
|
||||
* © 2022, Peter Cole. All rights reserved.
|
||||
* © 2022, Harald Barth. All rights reserved.
|
||||
*
|
||||
* This file is part of EX-CommandStation
|
||||
*
|
||||
@@ -23,10 +22,13 @@
|
||||
* This device driver will configure the device on startup, along with
|
||||
* interacting with the device for all input/output duties.
|
||||
*
|
||||
* To create EX-IOExpander devices, these are defined in myAutomation.h:
|
||||
* To create EX-IOExpander devices, these are defined in myHal.cpp:
|
||||
* (Note the device driver is included by default)
|
||||
*
|
||||
* HAL(EXIOExpander,800,18,0x65)
|
||||
* void halSetup() {
|
||||
* // EXIOExpander::create(vpin, num_vpins, i2c_address);
|
||||
* EXIOExpander::create(800, 18, 0x65);
|
||||
* }
|
||||
*
|
||||
* All pins on an EX-IOExpander device are allocated according to the pin map for the specific
|
||||
* device in use. There is no way for the device driver to sanity check pins are used for the
|
||||
@@ -96,45 +98,25 @@ private:
|
||||
_numAnaloguePins = receiveBuffer[2];
|
||||
|
||||
// See if we already have suitable buffers assigned
|
||||
if (_numDigitalPins>0) {
|
||||
size_t digitalBytesNeeded = (_numDigitalPins + 7) / 8;
|
||||
if (_digitalPinBytes < digitalBytesNeeded) {
|
||||
// Not enough space, free any existing buffer and allocate a new one
|
||||
if (_digitalPinBytes > 0) free(_digitalInputStates);
|
||||
if ((_digitalInputStates = (byte*) calloc(digitalBytesNeeded, 1)) != NULL) {
|
||||
_digitalPinBytes = digitalBytesNeeded;
|
||||
} else {
|
||||
DIAG(F("EX-IOExpander I2C:%s ERROR alloc %d bytes"), _I2CAddress.toString(), digitalBytesNeeded);
|
||||
_deviceState = DEVSTATE_FAILED;
|
||||
_digitalPinBytes = 0;
|
||||
return;
|
||||
}
|
||||
}
|
||||
size_t digitalBytesNeeded = (_numDigitalPins + 7) / 8;
|
||||
if (_digitalPinBytes < digitalBytesNeeded) {
|
||||
// Not enough space, free any existing buffer and allocate a new one
|
||||
if (_digitalPinBytes > 0) free(_digitalInputStates);
|
||||
_digitalInputStates = (byte*) calloc(_digitalPinBytes, 1);
|
||||
_digitalPinBytes = digitalBytesNeeded;
|
||||
}
|
||||
|
||||
if (_numAnaloguePins>0) {
|
||||
size_t analogueBytesNeeded = _numAnaloguePins * 2;
|
||||
if (_analoguePinBytes < analogueBytesNeeded) {
|
||||
// Free any existing buffers and allocate new ones.
|
||||
if (_analoguePinBytes > 0) {
|
||||
free(_analogueInputBuffer);
|
||||
free(_analogueInputStates);
|
||||
free(_analoguePinMap);
|
||||
}
|
||||
_analogueInputStates = (uint8_t*) calloc(analogueBytesNeeded, 1);
|
||||
_analogueInputBuffer = (uint8_t*) calloc(analogueBytesNeeded, 1);
|
||||
_analoguePinMap = (uint8_t*) calloc(_numAnaloguePins, 1);
|
||||
if (_analogueInputStates != NULL &&
|
||||
_analogueInputBuffer != NULL &&
|
||||
_analoguePinMap != NULL) {
|
||||
_analoguePinBytes = analogueBytesNeeded;
|
||||
} else {
|
||||
DIAG(F("EX-IOExpander I2C:%s ERROR alloc analog pin bytes"), _I2CAddress.toString());
|
||||
_deviceState = DEVSTATE_FAILED;
|
||||
_analoguePinBytes = 0;
|
||||
return;
|
||||
}
|
||||
size_t analogueBytesNeeded = _numAnaloguePins * 2;
|
||||
if (_analoguePinBytes < analogueBytesNeeded) {
|
||||
// Free any existing buffers and allocate new ones.
|
||||
if (_analoguePinBytes > 0) {
|
||||
free(_analogueInputBuffer);
|
||||
free(_analogueInputStates);
|
||||
free(_analoguePinMap);
|
||||
}
|
||||
_analogueInputStates = (uint8_t*) calloc(analogueBytesNeeded, 1);
|
||||
_analogueInputBuffer = (uint8_t*) calloc(analogueBytesNeeded, 1);
|
||||
_analoguePinMap = (uint8_t*) calloc(_numAnaloguePins, 1);
|
||||
_analoguePinBytes = analogueBytesNeeded;
|
||||
}
|
||||
} else {
|
||||
DIAG(F("EX-IOExpander I2C:%s ERROR configuring device"), _I2CAddress.toString());
|
||||
@@ -142,8 +124,8 @@ private:
|
||||
return;
|
||||
}
|
||||
}
|
||||
// We now need to retrieve the analogue pin map if there are analogue pins
|
||||
if (status == I2C_STATUS_OK && _numAnaloguePins>0) {
|
||||
// We now need to retrieve the analogue pin map
|
||||
if (status == I2C_STATUS_OK) {
|
||||
commandBuffer[0] = EXIOINITA;
|
||||
status = I2CManager.read(_I2CAddress, _analoguePinMap, _numAnaloguePins, commandBuffer, 1);
|
||||
}
|
||||
@@ -257,7 +239,7 @@ private:
|
||||
|
||||
// If we're not doing anything now, check to see if a new input transfer is due.
|
||||
if (_readState == RDS_IDLE) {
|
||||
if (currentMicros - _lastDigitalRead > _digitalRefresh && _numDigitalPins>0) { // Delay for digital read refresh
|
||||
if (currentMicros - _lastDigitalRead > _digitalRefresh) { // Delay for digital read refresh
|
||||
// Issue new read request for digital states. As the request is non-blocking, the buffer has to
|
||||
// be allocated from heap (object state).
|
||||
_readCommandBuffer[0] = EXIORDD;
|
||||
@@ -265,7 +247,7 @@ private:
|
||||
// non-blocking read
|
||||
_lastDigitalRead = currentMicros;
|
||||
_readState = RDS_DIGITAL;
|
||||
} else if (currentMicros - _lastAnalogueRead > _analogueRefresh && _numAnaloguePins>0) { // Delay for analogue read refresh
|
||||
} else if (currentMicros - _lastAnalogueRead > _analogueRefresh) { // Delay for analogue read refresh
|
||||
// Issue new read for analogue input states
|
||||
_readCommandBuffer[0] = EXIORDAN;
|
||||
I2CManager.read(_I2CAddress, _analogueInputBuffer,
|
||||
@@ -380,14 +362,14 @@ private:
|
||||
uint8_t _minorVer = 0;
|
||||
uint8_t _patchVer = 0;
|
||||
|
||||
uint8_t* _digitalInputStates = NULL;
|
||||
uint8_t* _analogueInputStates = NULL;
|
||||
uint8_t* _analogueInputBuffer = NULL; // buffer for I2C input transfers
|
||||
uint8_t* _digitalInputStates;
|
||||
uint8_t* _analogueInputStates;
|
||||
uint8_t* _analogueInputBuffer; // buffer for I2C input transfers
|
||||
uint8_t _readCommandBuffer[1];
|
||||
|
||||
uint8_t _digitalPinBytes = 0; // Size of allocated memory buffer (may be longer than needed)
|
||||
uint8_t _analoguePinBytes = 0; // Size of allocated memory buffer (may be longer than needed)
|
||||
uint8_t* _analoguePinMap = NULL;
|
||||
uint8_t _digitalPinBytes = 0; // Size of allocated memory buffer (may be longer than needed)
|
||||
uint8_t _analoguePinBytes = 0; // Size of allocated memory buffers (may be longer than needed)
|
||||
uint8_t* _analoguePinMap;
|
||||
I2CRB _i2crb;
|
||||
|
||||
enum {RDS_IDLE, RDS_DIGITAL, RDS_ANALOGUE}; // Read operation states
|
||||
|
@@ -1,98 +0,0 @@
|
||||
/*
|
||||
* © 2023, Chris Harlow. All rights reserved.
|
||||
* © 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_trainbrains_h
|
||||
#define io_trainbrains_h
|
||||
|
||||
#include "IO_GPIOBase.h"
|
||||
#include "FSH.h"
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/*
|
||||
* IODevice subclass for trainbrains 3-block occupancy detector.
|
||||
* For details see http://trainbrains.eu
|
||||
*/
|
||||
|
||||
enum TrackUnoccupancy
|
||||
{
|
||||
TRACK_UNOCCUPANCY_UNKNOWN = 0,
|
||||
TRACK_OCCUPIED = 1,
|
||||
TRACK_UNOCCUPIED = 2
|
||||
};
|
||||
|
||||
class Trainbrains02 : public GPIOBase<uint16_t> {
|
||||
public:
|
||||
static void create(VPIN vpin, uint8_t nPins, I2CAddress i2cAddress) {
|
||||
if (checkNoOverlap(vpin, nPins, i2cAddress)) new Trainbrains02(vpin, nPins, i2cAddress);
|
||||
}
|
||||
|
||||
private:
|
||||
// Constructor
|
||||
Trainbrains02(VPIN vpin, uint8_t nPins, I2CAddress i2cAddress, int interruptPin=-1)
|
||||
: GPIOBase<uint16_t>((FSH *)F("Trainbrains02"), vpin, nPins, i2cAddress, interruptPin)
|
||||
{
|
||||
requestBlock.setRequestParams(_I2CAddress, inputBuffer, sizeof(inputBuffer),
|
||||
outputBuffer, sizeof(outputBuffer));
|
||||
|
||||
outputBuffer[0] = (uint8_t)_I2CAddress; // strips away the mux part.
|
||||
outputBuffer[1] =14;
|
||||
outputBuffer[2] =1;
|
||||
outputBuffer[3] =0; // This is the channel updated at each poling call
|
||||
outputBuffer[4] =0;
|
||||
outputBuffer[5] =0;
|
||||
outputBuffer[6] =0;
|
||||
outputBuffer[7] =0;
|
||||
outputBuffer[8] =0;
|
||||
outputBuffer[9] =0;
|
||||
}
|
||||
|
||||
void _writeGpioPort() override {}
|
||||
|
||||
void _readGpioPort(bool immediate) override {
|
||||
// cycle channel on device each time
|
||||
outputBuffer[3]=channelInProgress+1; // 1-origin
|
||||
channelInProgress++;
|
||||
if(channelInProgress>=_nPins) channelInProgress=0;
|
||||
|
||||
if (immediate) {
|
||||
_processCompletion(I2CManager.read(_I2CAddress, inputBuffer, sizeof(inputBuffer),
|
||||
outputBuffer, sizeof(outputBuffer)));
|
||||
} 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) inputBuffer[6]=TRACK_UNOCCUPANCY_UNKNOWN;
|
||||
if (inputBuffer[6] == TRACK_UNOCCUPIED ) _portInputState |= 0x01 <<channelInProgress;
|
||||
else _portInputState &= ~(0x01 <<channelInProgress);
|
||||
}
|
||||
|
||||
uint8_t channelInProgress=0;
|
||||
uint8_t outputBuffer[10];
|
||||
uint8_t inputBuffer[10];
|
||||
|
||||
};
|
||||
|
||||
#endif
|
@@ -1,57 +0,0 @@
|
||||
/*
|
||||
* © 2024 Vincent Hamp and Chris Harlow
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
|
||||
/* Reader be aware:
|
||||
This function implements the _hk data type so that a string keyword
|
||||
is hashed to the same value as the DCCEXParser uses to hash incoming
|
||||
keywords.
|
||||
Thus "MAIN"_hk generates exactly the same run time vakue
|
||||
as const int16_t HASH_KEYWORD_MAIN=11339
|
||||
*/
|
||||
#ifndef KeywordHAsher_h
|
||||
#define KeywordHasher_h
|
||||
|
||||
#include <Arduino.h>
|
||||
constexpr uint16_t CompiletimeKeywordHasher(const char * sv, uint16_t running=0) {
|
||||
return (*sv==0) ? running : CompiletimeKeywordHasher(sv+1,
|
||||
(*sv >= '0' && *sv <= '9')
|
||||
? (10*running+*sv-'0') // Numeric hash
|
||||
: ((running << 5) + running) ^ *sv
|
||||
); //
|
||||
}
|
||||
|
||||
constexpr int16_t operator""_hk(const char * keyword, size_t len)
|
||||
{
|
||||
return (int16_t) CompiletimeKeywordHasher(keyword,len*0);
|
||||
}
|
||||
|
||||
/* Some historical values for testing:
|
||||
const int16_t HASH_KEYWORD_MAIN = 11339;
|
||||
const int16_t HASH_KEYWORD_SLOW = -17209;
|
||||
const int16_t HASH_KEYWORD_SPEED28 = -17064;
|
||||
const int16_t HASH_KEYWORD_SPEED128 = 25816;
|
||||
*/
|
||||
|
||||
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");
|
||||
#endif
|
@@ -5,6 +5,7 @@
|
||||
* © 2020-2023 Harald Barth
|
||||
* © 2020-2021 Chris Harlow
|
||||
* © 2023 Colin Murdoch
|
||||
* © 2023 Travis Farmer
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
@@ -57,6 +58,7 @@ MotorDriver::MotorDriver(int16_t power_pin, byte signal_pin, byte signal_pin2, i
|
||||
getFastPin(F("SIG"),signalPin,fastSignalPin);
|
||||
pinMode(signalPin, OUTPUT);
|
||||
|
||||
#ifndef ARDUINO_GIGA // no giga
|
||||
fastSignalPin.shadowinout = NULL;
|
||||
if (HAVE_PORTA(fastSignalPin.inout == &PORTA)) {
|
||||
DIAG(F("Found PORTA pin %d"),signalPin);
|
||||
@@ -88,13 +90,14 @@ MotorDriver::MotorDriver(int16_t power_pin, byte signal_pin, byte signal_pin2, i
|
||||
fastSignalPin.shadowinout = fastSignalPin.inout;
|
||||
fastSignalPin.inout = &shadowPORTF;
|
||||
}
|
||||
|
||||
#endif // giga
|
||||
signalPin2=signal_pin2;
|
||||
if (signalPin2!=UNUSED_PIN) {
|
||||
dualSignal=true;
|
||||
getFastPin(F("SIG2"),signalPin2,fastSignalPin2);
|
||||
pinMode(signalPin2, OUTPUT);
|
||||
|
||||
#ifndef ARDUINO_GIGA // no giga
|
||||
fastSignalPin2.shadowinout = NULL;
|
||||
if (HAVE_PORTA(fastSignalPin2.inout == &PORTA)) {
|
||||
DIAG(F("Found PORTA pin %d"),signalPin2);
|
||||
@@ -126,6 +129,7 @@ MotorDriver::MotorDriver(int16_t power_pin, byte signal_pin, byte signal_pin2, i
|
||||
fastSignalPin2.shadowinout = fastSignalPin2.inout;
|
||||
fastSignalPin2.inout = &shadowPORTF;
|
||||
}
|
||||
#endif // giga
|
||||
}
|
||||
else dualSignal=false;
|
||||
|
||||
@@ -501,8 +505,16 @@ unsigned int MotorDriver::mA2raw( unsigned int mA) {
|
||||
return (int32_t)mA * senseScale / senseFactorInternal;
|
||||
}
|
||||
|
||||
|
||||
void MotorDriver::getFastPin(const FSH* type,int pin, bool input, FASTPIN & result) {
|
||||
// DIAG(F("MotorDriver %S Pin=%d,"),type,pin);
|
||||
#if defined(ARDUINO_GIGA) // yes giga
|
||||
(void)type;
|
||||
(void)input; // no warnings please
|
||||
|
||||
result = pin;
|
||||
|
||||
#else // no giga
|
||||
(void) type; // avoid compiler warning if diag not used above.
|
||||
#if defined(ARDUINO_ARCH_SAMD)
|
||||
PortGroup *port = digitalPinToPort(pin);
|
||||
@@ -517,6 +529,7 @@ void MotorDriver::getFastPin(const FSH* type,int pin, bool input, FASTPIN & res
|
||||
result.inout = portOutputRegister(port);
|
||||
result.maskHIGH = digitalPinToBitMask(pin);
|
||||
result.maskLOW = ~result.maskHIGH;
|
||||
#endif // giga
|
||||
// DIAG(F(" port=0x%x, inoutpin=0x%x, isinput=%d, mask=0x%x"),port, result.inout,input,result.maskHIGH);
|
||||
}
|
||||
|
||||
@@ -605,10 +618,6 @@ 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))){
|
||||
DIAG(F("TRACK %c INVERT"), trackno + 'A');
|
||||
invertOutput();
|
||||
}
|
||||
break;
|
||||
}
|
||||
// all well
|
||||
@@ -680,10 +689,8 @@ void MotorDriver::checkPowerOverload(bool useProgLimit, byte trackno) {
|
||||
power_sample_overload_wait *= 2;
|
||||
if (power_sample_overload_wait > POWER_SAMPLE_RETRY_MAX)
|
||||
power_sample_overload_wait = POWER_SAMPLE_RETRY_MAX;
|
||||
#ifdef EXRAIL_ACTIVE
|
||||
DIAG(F("Calling EXRAIL"));
|
||||
RMFT2::powerEvent(trackno, true); // Tell EXRAIL we have an overload
|
||||
#endif
|
||||
// power on test
|
||||
DIAG(F("TRACK %c POWER RESTORE (after %4M)"), trackno + 'A', mslpc);
|
||||
setPower(POWERMODE::ALERT);
|
||||
|
@@ -3,7 +3,8 @@
|
||||
* © 2021 Mike S
|
||||
* © 2021 Fred Decker
|
||||
* © 2020 Chris Harlow
|
||||
* © 2022,2023 Harald Barth
|
||||
* © 2022 Harald Barth
|
||||
* © 2023 Travis Farmer
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
@@ -28,21 +29,23 @@
|
||||
#include "DCCTimer.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)
|
||||
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, 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};
|
||||
TRACK_MODE_DC = 8, TRACK_MODE_DCX = 16, TRACK_MODE_EXT = 32};
|
||||
#if defined(ARDUINO_GIGA) // yes giga
|
||||
|
||||
#define setHIGH(fastpin) digitalWrite(fastpin,1)
|
||||
#define setLOW(fastpin) digitalWrite(fastpin,0)
|
||||
#else // no giga
|
||||
#define setHIGH(fastpin) *fastpin.inout |= fastpin.maskHIGH
|
||||
#define setLOW(fastpin) *fastpin.inout &= fastpin.maskLOW
|
||||
#endif // giga
|
||||
#if defined(ARDUINO_GIGA) // yes giga
|
||||
#define isHIGH(fastpin) ((PinStatus)digitalRead(fastpin)==1)
|
||||
#define isLOW(fastpin) ((PinStatus)digitalRead(fastpin)==0)
|
||||
#else // no giga
|
||||
#define isHIGH(fastpin) (*fastpin.inout & fastpin.maskHIGH)
|
||||
#define isLOW(fastpin) (!isHIGH(fastpin))
|
||||
|
||||
#endif // giga
|
||||
#define TOKENPASTE(x, y) x ## y
|
||||
#define TOKENPASTE2(x, y) TOKENPASTE(x, y)
|
||||
|
||||
@@ -124,12 +127,19 @@ typedef uint32_t portreg_t;
|
||||
typedef uint8_t portreg_t;
|
||||
#endif
|
||||
|
||||
#if defined(ARDUINO_GIGA) // yes giga
|
||||
typedef int FASTPIN;
|
||||
|
||||
|
||||
#else // no giga
|
||||
struct FASTPIN {
|
||||
volatile portreg_t *inout;
|
||||
portreg_t maskHIGH;
|
||||
portreg_t maskLOW;
|
||||
volatile portreg_t *shadowinout;
|
||||
};
|
||||
#endif // giga
|
||||
|
||||
// The port registers that are shadowing
|
||||
// the real port registers. These are
|
||||
// defined in Motordriver.cpp
|
||||
@@ -155,11 +165,13 @@ class MotorDriver {
|
||||
// otherwise the call from interrupt context can undo whatever we do
|
||||
// from outside interrupt
|
||||
void setBrake( bool on, bool interruptContext=false);
|
||||
#if defined(ARDUINO_GIGA) // yes giga
|
||||
__attribute__((always_inline)) inline void setSignal( bool high) {
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
if (invertPhase)
|
||||
high = !high;
|
||||
#endif
|
||||
digitalWrite(signalPin, high);
|
||||
if (dualSignal) digitalWrite(signalPin2, !high);
|
||||
};
|
||||
#else // no giga
|
||||
__attribute__((always_inline)) inline void setSignal( bool high) {
|
||||
if (trackPWM) {
|
||||
DCCTimer::setPWM(signalPin,high);
|
||||
}
|
||||
@@ -174,17 +186,12 @@ class MotorDriver {
|
||||
}
|
||||
}
|
||||
};
|
||||
#endif // giga
|
||||
inline void enableSignal(bool on) {
|
||||
if (on)
|
||||
pinMode(signalPin, OUTPUT);
|
||||
else
|
||||
pinMode(signalPin, INPUT);
|
||||
if (signalPin2 != UNUSED_PIN) {
|
||||
if (on)
|
||||
pinMode(signalPin2, OUTPUT);
|
||||
else
|
||||
pinMode(signalPin2, INPUT);
|
||||
}
|
||||
};
|
||||
inline pinpair getSignalPin() { return pinpair(signalPin,signalPin2); };
|
||||
void setDCSignal(byte speedByte);
|
||||
@@ -201,6 +208,12 @@ class MotorDriver {
|
||||
int getCurrentRaw(bool fromISR=false);
|
||||
unsigned int raw2mA( int raw);
|
||||
unsigned int mA2raw( unsigned int mA);
|
||||
#if defined(ARDUINO_GIGA) // yes giga
|
||||
inline bool digitalPinHasPWM(int pin) {
|
||||
if (pin!=UNUSED_PIN && pin>=2 && pin<=13) return true;
|
||||
else return false;
|
||||
}
|
||||
#endif // giga
|
||||
inline bool brakeCanPWM() {
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
return (brakePin != UNUSED_PIN); // This was just (true) but we probably do need to check for UNUSED_PIN!
|
||||
@@ -249,32 +262,6 @@ class MotorDriver {
|
||||
#endif
|
||||
inline void setMode(TRACK_MODE m) {
|
||||
trackMode = m;
|
||||
invertOutput(trackMode & TRACK_MODE_INV);
|
||||
};
|
||||
inline void invertOutput() { // toggles output inversion
|
||||
invertPhase = !invertPhase;
|
||||
invertOutput(invertPhase);
|
||||
};
|
||||
inline void invertOutput(bool b) { // sets output inverted or not
|
||||
if (b)
|
||||
invertPhase = 1;
|
||||
else
|
||||
invertPhase = 0;
|
||||
#if defined(ARDUINO_ARCH_ESP32)
|
||||
pinpair p = getSignalPin();
|
||||
uint32_t *outreg = (uint32_t *)(GPIO_FUNC0_OUT_SEL_CFG_REG + 4*p.pin);
|
||||
if (invertPhase) // set or clear the invert bit in the gpio out register
|
||||
*outreg |= ((uint32_t)0x1 << GPIO_FUNC0_OUT_INV_SEL_S);
|
||||
else
|
||||
*outreg &= ~((uint32_t)0x1 << GPIO_FUNC0_OUT_INV_SEL_S);
|
||||
if (p.invpin != UNUSED_PIN) {
|
||||
outreg = (uint32_t *)(GPIO_FUNC0_OUT_SEL_CFG_REG + 4*p.invpin);
|
||||
if (invertPhase) // clear or set the invert bit in the gpio out register
|
||||
*outreg &= ~((uint32_t)0x1 << GPIO_FUNC0_OUT_INV_SEL_S);
|
||||
else
|
||||
*outreg |= ((uint32_t)0x1 << GPIO_FUNC0_OUT_INV_SEL_S);
|
||||
}
|
||||
#endif
|
||||
};
|
||||
inline TRACK_MODE getMode() {
|
||||
return trackMode;
|
||||
@@ -306,7 +293,7 @@ class MotorDriver {
|
||||
bool invertBrake; // brake pin passed as negative means pin is inverted
|
||||
bool invertPower; // power pin passed as negative means pin is inverted
|
||||
bool invertFault; // fault pin passed as negative means pin is inverted
|
||||
bool invertPhase = 0; // phase of out pin is inverted
|
||||
|
||||
// Raw to milliamp conversion factors avoiding float data types.
|
||||
// Milliamps=rawADCreading * sensefactorInternal / senseScale
|
||||
//
|
||||
|
@@ -111,15 +111,14 @@ void SerialManager::loop2() {
|
||||
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;
|
||||
}
|
||||
else if (ch == '>') {
|
||||
buffer[bufferLength] = '\0';
|
||||
DCCEXParser::parse(serial, buffer, NULL);
|
||||
inCommandPayload = false;
|
||||
break;
|
||||
}
|
||||
else if (inCommandPayload) {
|
||||
if (bufferLength < (COMMAND_BUFFER_SIZE-1)) buffer[bufferLength++] = ch;
|
||||
}
|
||||
}
|
||||
|
||||
|
@@ -19,7 +19,6 @@
|
||||
#include "StringFormatter.h"
|
||||
#include <stdarg.h>
|
||||
#include "DisplayInterface.h"
|
||||
#include "CommandDistributor.h"
|
||||
|
||||
bool Diag::ACK=false;
|
||||
bool Diag::CMD=false;
|
||||
@@ -39,28 +38,13 @@ void StringFormatter::diag( const FSH* input...) {
|
||||
|
||||
void StringFormatter::lcd(byte row, const FSH* input...) {
|
||||
va_list args;
|
||||
#ifndef DISABLE_VDPY
|
||||
Print * virtualLCD=CommandDistributor::getVirtualLCDSerial(0,row);
|
||||
#else
|
||||
Print * virtualLCD=NULL;
|
||||
#endif
|
||||
|
||||
// Issue the LCD as a diag first
|
||||
// Unless the same serial is asking for the virtual @ respomnse
|
||||
if (virtualLCD!=&USB_SERIAL) {
|
||||
send(&USB_SERIAL,F("<* LCD%d:"),row);
|
||||
va_start(args, input);
|
||||
send2(&USB_SERIAL,input,args);
|
||||
send(&USB_SERIAL,F(" *>\n"));
|
||||
}
|
||||
send(&USB_SERIAL,F("<* LCD%d:"),row);
|
||||
va_start(args, input);
|
||||
send2(&USB_SERIAL,input,args);
|
||||
send(&USB_SERIAL,F(" *>\n"));
|
||||
|
||||
#ifndef DISABLE_VDPY
|
||||
// send to virtual LCD collector (if any)
|
||||
if (virtualLCD) {
|
||||
va_start(args, input);
|
||||
send2(virtualLCD,input,args);
|
||||
CommandDistributor::commitVirtualLCDSerial();
|
||||
}
|
||||
#endif
|
||||
DisplayInterface::setRow(row);
|
||||
va_start(args, input);
|
||||
send2(DisplayInterface::getDisplayHandler(),input,args);
|
||||
@@ -68,16 +52,6 @@ void StringFormatter::lcd(byte row, const FSH* input...) {
|
||||
|
||||
void StringFormatter::lcd2(uint8_t display, byte row, const FSH* input...) {
|
||||
va_list args;
|
||||
|
||||
// send to virtual LCD collector (if any)
|
||||
#ifndef DISABLE_VDPY
|
||||
Print * virtualLCD=CommandDistributor::getVirtualLCDSerial(display,row);
|
||||
if (virtualLCD) {
|
||||
va_start(args, input);
|
||||
send2(virtualLCD,input,args);
|
||||
CommandDistributor::commitVirtualLCDSerial();
|
||||
}
|
||||
#endif
|
||||
|
||||
DisplayInterface::setRow(display, row);
|
||||
va_start(args, input);
|
||||
@@ -256,3 +230,4 @@ void StringFormatter::printHex(Print * stream,uint16_t value) {
|
||||
result[4]='\0';
|
||||
stream->print(result);
|
||||
}
|
||||
|
@@ -54,5 +54,6 @@ class StringFormatter
|
||||
private:
|
||||
static void send2(Print * serial, const FSH* input,va_list args);
|
||||
static void printPadded(Print* stream, long value, byte width, bool formatLeft);
|
||||
|
||||
};
|
||||
#endif
|
||||
|
359
TrackManager.cpp
359
TrackManager.cpp
@@ -1,6 +1,6 @@
|
||||
/*
|
||||
* © 2022 Chris Harlow
|
||||
* © 2022,2023 Harald Barth
|
||||
* © 2022 Harald Barth
|
||||
* © 2023 Colin Murdoch
|
||||
* All rights reserved.
|
||||
*
|
||||
@@ -28,7 +28,6 @@
|
||||
#include "DIAG.h"
|
||||
#include "CommandDistributor.h"
|
||||
#include "DCCEXParser.h"
|
||||
#include "KeywordHasher.h"
|
||||
// Virtualised Motor shield multi-track hardware Interface
|
||||
#define FOR_EACH_TRACK(t) for (byte t=0;t<=lastTrack;t++)
|
||||
|
||||
@@ -36,10 +35,21 @@
|
||||
FOR_EACH_TRACK(t) \
|
||||
if (track[t]->getMode()==findmode) \
|
||||
track[t]->function;
|
||||
#ifndef DISABLE_PROG
|
||||
const int16_t HASH_KEYWORD_PROG = -29718;
|
||||
#endif
|
||||
const int16_t HASH_KEYWORD_MAIN = 11339;
|
||||
const int16_t HASH_KEYWORD_OFF = 22479;
|
||||
const int16_t HASH_KEYWORD_NONE = -26550;
|
||||
const int16_t HASH_KEYWORD_DC = 2183;
|
||||
const int16_t HASH_KEYWORD_DCX = 6463; // DC reversed polarity
|
||||
const int16_t HASH_KEYWORD_EXT = 8201; // External DCC signal
|
||||
const int16_t HASH_KEYWORD_A = 65; // parser makes single chars the ascii.
|
||||
|
||||
MotorDriver * TrackManager::track[MAX_TRACKS];
|
||||
int16_t TrackManager::trackDCAddr[MAX_TRACKS];
|
||||
|
||||
POWERMODE TrackManager::mainPowerGuess=POWERMODE::OFF;
|
||||
byte TrackManager::lastTrack=0;
|
||||
bool TrackManager::progTrackSyncMain=false;
|
||||
bool TrackManager::progTrackBoosted=false;
|
||||
@@ -77,7 +87,7 @@ void TrackManager::sampleCurrent() {
|
||||
if (!waiting) {
|
||||
// look for a valid track to sample or until we are around
|
||||
while (true) {
|
||||
if (track[tr]->getMode() & ( TRACK_MODE_MAIN|TRACK_MODE_PROG|TRACK_MODE_DC|TRACK_MODE_BOOST|TRACK_MODE_EXT )) {
|
||||
if (track[tr]->getMode() & ( TRACK_MODE_MAIN|TRACK_MODE_PROG|TRACK_MODE_DC|TRACK_MODE_DCX|TRACK_MODE_EXT )) {
|
||||
track[tr]->startCurrentFromHW();
|
||||
// for scope debug track[1]->setBrake(1);
|
||||
waiting = true;
|
||||
@@ -187,20 +197,17 @@ void TrackManager::setPROGSignal( bool on) {
|
||||
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);
|
||||
if (track[t]->getMode()==TRACK_MODE_DC) track[t]->setDCSignal(speedbyte);
|
||||
else if (track[t]->getMode()==TRACK_MODE_DCX) track[t]->setDCSignal(speedbyte ^ 128);
|
||||
}
|
||||
}
|
||||
|
||||
bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr) {
|
||||
if (trackToSet>lastTrack || track[trackToSet]==NULL) return false;
|
||||
|
||||
// Remember track mode we came from for later
|
||||
TRACK_MODE oldmode = track[trackToSet]->getMode();
|
||||
|
||||
//DIAG(F("Track=%c Mode=%d"),trackToSet+'A', mode);
|
||||
// DC tracks require a motorDriver that can set brake!
|
||||
if (mode & TRACK_MODE_DC) {
|
||||
if (mode==TRACK_MODE_DC || mode==TRACK_MODE_DCX) {
|
||||
#if defined(ARDUINO_AVR_UNO)
|
||||
DIAG(F("Uno has no PWM timers available for DC"));
|
||||
return false;
|
||||
@@ -216,41 +223,25 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
|
||||
pinpair p = track[trackToSet]->getSignalPin();
|
||||
//DIAG(F("Track=%c remove pin %d"),trackToSet+'A', p.pin);
|
||||
gpio_reset_pin((gpio_num_t)p.pin);
|
||||
pinMode(p.pin, OUTPUT); // gpio_reset_pin may reset to input
|
||||
if (p.invpin != UNUSED_PIN) {
|
||||
//DIAG(F("Track=%c remove ^pin %d"),trackToSet+'A', p.invpin);
|
||||
gpio_reset_pin((gpio_num_t)p.invpin);
|
||||
pinMode(p.invpin, OUTPUT); // gpio_reset_pin may reset to input
|
||||
}
|
||||
#ifdef BOOSTER_INPUT
|
||||
if (mode & TRACK_MODE_BOOST) {
|
||||
//DIAG(F("Track=%c mode boost pin %d"),trackToSet+'A', p.pin);
|
||||
pinMode(BOOSTER_INPUT, INPUT);
|
||||
gpio_matrix_in(26, SIG_IN_FUNC228_IDX, false); //pads 224 to 228 available as loopback
|
||||
gpio_matrix_out(p.pin, SIG_IN_FUNC228_IDX, false, false);
|
||||
if (p.invpin != UNUSED_PIN) {
|
||||
gpio_matrix_out(p.invpin, SIG_IN_FUNC228_IDX, true /*inverted*/, false);
|
||||
}
|
||||
} else // elseif clause continues
|
||||
#endif
|
||||
if (mode & (TRACK_MODE_MAIN | TRACK_MODE_PROG | TRACK_MODE_DC)) {
|
||||
// gpio_reset_pin may reset to input
|
||||
pinMode(p.pin, OUTPUT);
|
||||
if (p.invpin != UNUSED_PIN)
|
||||
pinMode(p.invpin, OUTPUT);
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifndef DISABLE_PROG
|
||||
if (mode & TRACK_MODE_PROG) {
|
||||
if (mode==TRACK_MODE_PROG) {
|
||||
#else
|
||||
if (false) {
|
||||
#endif
|
||||
// only allow 1 track to be prog
|
||||
FOR_EACH_TRACK(t)
|
||||
if ( (track[t]->getMode() & TRACK_MODE_PROG) && t != trackToSet) {
|
||||
if (track[t]->getMode()==TRACK_MODE_PROG && t != trackToSet) {
|
||||
track[t]->setPower(POWERMODE::OFF);
|
||||
track[t]->setMode(TRACK_MODE_NONE);
|
||||
track[t]->makeProgTrack(false); // revoke prog track special handling
|
||||
streamTrackState(NULL,t);
|
||||
streamTrackState(NULL,t);
|
||||
}
|
||||
track[trackToSet]->makeProgTrack(true); // set for prog track special handling
|
||||
} else {
|
||||
@@ -258,25 +249,22 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
|
||||
}
|
||||
track[trackToSet]->setMode(mode);
|
||||
trackDCAddr[trackToSet]=dcAddr;
|
||||
streamTrackState(NULL,trackToSet);
|
||||
|
||||
// When a track is switched, we must clear any side effects of its previous
|
||||
// state, otherwise trains run away or just dont move.
|
||||
|
||||
// This can be done BEFORE the PWM-Timer evaluation (methinks)
|
||||
if (!(mode & TRACK_MODE_DC)) {
|
||||
if (!(mode==TRACK_MODE_DC || mode==TRACK_MODE_DCX)) {
|
||||
// DCC tracks need to have set the PWM to zero or they will not work.
|
||||
track[trackToSet]->detachDCSignal();
|
||||
track[trackToSet]->setBrake(false);
|
||||
}
|
||||
|
||||
// BOOST:
|
||||
// Leave it as is
|
||||
// otherwise:
|
||||
// EXT is a special case where the signal pin is
|
||||
// turned off. So unless that is set, the signal
|
||||
// pin should be turned on
|
||||
if (!(mode & TRACK_MODE_BOOST))
|
||||
track[trackToSet]->enableSignal(!(mode & TRACK_MODE_EXT));
|
||||
// EXT is a special case where the signal pin is
|
||||
// turned off. So unless that is set, the signal
|
||||
// pin should be turned on
|
||||
track[trackToSet]->enableSignal(mode != TRACK_MODE_EXT);
|
||||
|
||||
#ifndef ARDUINO_ARCH_ESP32
|
||||
// re-evaluate HighAccuracy mode
|
||||
@@ -286,7 +274,7 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
|
||||
// DC tracks must not have the DCC PWM switched on
|
||||
// so we globally turn it off if one of the PWM
|
||||
// capable tracks is now DC or DCX.
|
||||
if (track[t]->getMode() & TRACK_MODE_DC) {
|
||||
if (track[t]->getMode()==TRACK_MODE_DC || track[t]->getMode()==TRACK_MODE_DCX) {
|
||||
if (track[t]->isPWMCapable()) {
|
||||
canDo=false; // this track is capable but can not run PWM
|
||||
break; // in this mode, so abort and prevent globally below
|
||||
@@ -294,7 +282,7 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
|
||||
track[t]->trackPWM=false; // this track sure can not run with PWM
|
||||
//DIAG(F("Track %c trackPWM 0 (not capable)"), t+'A');
|
||||
}
|
||||
} else if (track[t]->getMode() & (TRACK_MODE_MAIN |TRACK_MODE_PROG)) {
|
||||
} else if (track[t]->getMode()==TRACK_MODE_MAIN || track[t]->getMode()==TRACK_MODE_PROG) {
|
||||
track[t]->trackPWM = track[t]->isPWMCapable(); // trackPWM is still a guess here
|
||||
//DIAG(F("Track %c trackPWM %d"), t+'A', track[t]->trackPWM);
|
||||
canDo &= track[t]->trackPWM;
|
||||
@@ -312,33 +300,32 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
|
||||
#else
|
||||
// For ESP32 we just reinitialize the DCC Waveform
|
||||
DCCWaveform::begin();
|
||||
// setMode() again AFTER Waveform::begin() of ESP32 fixes INVERTED signal
|
||||
track[trackToSet]->setMode(mode);
|
||||
#endif
|
||||
|
||||
// This block must be AFTER the PWM-Timer modifications
|
||||
if (mode & TRACK_MODE_DC) {
|
||||
if (mode==TRACK_MODE_DC || mode==TRACK_MODE_DCX) {
|
||||
// DC tracks need to be given speed of the throttle for that cab address
|
||||
// otherwise will not match other tracks on same cab.
|
||||
// This also needs to allow for inverted DCX
|
||||
applyDCSpeed(trackToSet);
|
||||
}
|
||||
|
||||
// Turn off power if we changed the mode of this track
|
||||
if (mode != oldmode)
|
||||
track[trackToSet]->setPower(POWERMODE::OFF);
|
||||
streamTrackState(NULL,trackToSet);
|
||||
|
||||
// Normal running tracks are set to the global power state
|
||||
track[trackToSet]->setPower(
|
||||
(mode==TRACK_MODE_MAIN || mode==TRACK_MODE_DC || mode==TRACK_MODE_DCX || mode==TRACK_MODE_EXT) ?
|
||||
mainPowerGuess : POWERMODE::OFF);
|
||||
//DIAG(F("TrackMode=%d"),mode);
|
||||
return true;
|
||||
}
|
||||
|
||||
void TrackManager::applyDCSpeed(byte t) {
|
||||
uint8_t speedByte=DCC::getThrottleSpeedByte(trackDCAddr[t]);
|
||||
if (track[t]->getMode()==TRACK_MODE_DCX)
|
||||
speedByte = speedByte ^ 128; // reverse direction bit
|
||||
track[t]->setDCSignal(speedByte);
|
||||
}
|
||||
|
||||
bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])
|
||||
bool TrackManager::parseJ(Print *stream, int16_t params, int16_t p[])
|
||||
{
|
||||
|
||||
if (params==0) { // <=> List track assignments
|
||||
@@ -348,97 +335,67 @@ bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])
|
||||
|
||||
}
|
||||
|
||||
p[0]-="A"_hk; // convert A... to 0....
|
||||
p[0]-=HASH_KEYWORD_A; // convert A... to 0....
|
||||
|
||||
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]==HASH_KEYWORD_MAIN) // <= id MAIN>
|
||||
return setTrackMode(p[0],TRACK_MODE_MAIN);
|
||||
|
||||
#ifndef DISABLE_PROG
|
||||
if (params==2 && p[1]=="PROG"_hk) // <= id PROG>
|
||||
if (params==2 && p[1]==HASH_KEYWORD_PROG) // <= id PROG>
|
||||
return setTrackMode(p[0],TRACK_MODE_PROG);
|
||||
#endif
|
||||
|
||||
if (params==2 && (p[1]=="OFF"_hk || p[1]=="NONE"_hk)) // <= id OFF> <= id NONE>
|
||||
if (params==2 && (p[1]==HASH_KEYWORD_OFF || p[1]==HASH_KEYWORD_NONE)) // <= id OFF> <= id NONE>
|
||||
return setTrackMode(p[0],TRACK_MODE_NONE);
|
||||
|
||||
if (params==2 && p[1]=="EXT"_hk) // <= id EXT>
|
||||
if (params==2 && p[1]==HASH_KEYWORD_EXT) // <= id EXT>
|
||||
return setTrackMode(p[0],TRACK_MODE_EXT);
|
||||
#ifdef BOOSTER_INPUT
|
||||
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>
|
||||
return setTrackMode(p[0], track[p[0]]->getMode() | TRACK_MODE_AUTOINV);
|
||||
|
||||
if (params==2 && p[1]=="INV"_hk) // <= id AUTO>
|
||||
return setTrackMode(p[0], track[p[0]]->getMode() | TRACK_MODE_INV);
|
||||
|
||||
if (params==3 && p[1]=="DC"_hk && p[2]>0) // <= id DC cab>
|
||||
if (params==3 && p[1]==HASH_KEYWORD_DC && 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]==HASH_KEYWORD_DCX && p[2]>0) // <= id DCX cab>
|
||||
return setTrackMode(p[0],TRACK_MODE_DCX,p[2]);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
const FSH* TrackManager::getModeName(TRACK_MODE tm) {
|
||||
const FSH *modename=F("---");
|
||||
|
||||
if (tm & TRACK_MODE_MAIN) {
|
||||
if(tm & TRACK_MODE_AUTOINV)
|
||||
modename=F("MAIN A");
|
||||
else if (tm & TRACK_MODE_INV)
|
||||
modename=F("MAIN I>\n");
|
||||
else
|
||||
modename=F("MAIN");
|
||||
}
|
||||
#ifndef DISABLE_PROG
|
||||
else if (tm & TRACK_MODE_PROG)
|
||||
modename=F("PROG");
|
||||
#endif
|
||||
else if (tm & TRACK_MODE_NONE)
|
||||
modename=F("NONE");
|
||||
else if(tm & TRACK_MODE_EXT)
|
||||
modename=F("EXT");
|
||||
else if(tm & TRACK_MODE_BOOST) {
|
||||
if(tm & TRACK_MODE_AUTOINV)
|
||||
modename=F("B A");
|
||||
else if (tm & TRACK_MODE_INV)
|
||||
modename=F("B I");
|
||||
else
|
||||
modename=F("B");
|
||||
}
|
||||
else if (tm & TRACK_MODE_DC) {
|
||||
if (tm & TRACK_MODE_INV)
|
||||
modename=F("DCX");
|
||||
else
|
||||
modename=F("DC");
|
||||
}
|
||||
return modename;
|
||||
}
|
||||
|
||||
// null stream means send to commandDistributor for broadcast
|
||||
void TrackManager::streamTrackState(Print* stream, byte t) {
|
||||
const FSH *format;
|
||||
|
||||
// null stream means send to commandDistributor for broadcast
|
||||
if (track[t]==NULL) return;
|
||||
TRACK_MODE tm = track[t]->getMode();
|
||||
if (tm & TRACK_MODE_DC)
|
||||
format=F("<= %c %S %d>\n");
|
||||
else
|
||||
format=F("<= %c %S>\n");
|
||||
|
||||
const FSH *modename=getModeName(tm);
|
||||
if (stream) { // null stream means send to commandDistributor for broadcast
|
||||
StringFormatter::send(stream,format,'A'+t, modename, trackDCAddr[t]);
|
||||
} else {
|
||||
CommandDistributor::broadcastTrackState(format,'A'+t, modename, trackDCAddr[t]);
|
||||
CommandDistributor::broadcastPower();
|
||||
auto format=F("");
|
||||
bool pstate = TrackManager::isPowerOn(t);
|
||||
|
||||
switch(track[t]->getMode()) {
|
||||
case TRACK_MODE_MAIN:
|
||||
if (pstate) {format=F("<= %c MAIN ON>\n");} else {format = F("<= %c MAIN OFF>\n");}
|
||||
break;
|
||||
#ifndef DISABLE_PROG
|
||||
case TRACK_MODE_PROG:
|
||||
if (pstate) {format=F("<= %c PROG ON>\n");} else {format=F("<= %c PROG OFF>\n");}
|
||||
break;
|
||||
#endif
|
||||
case TRACK_MODE_NONE:
|
||||
if (pstate) {format=F("<= %c NONE ON>\n");} else {format=F("<= %c NONE OFF>\n");}
|
||||
break;
|
||||
case TRACK_MODE_EXT:
|
||||
if (pstate) {format=F("<= %c EXT ON>\n");} else {format=F("<= %c EXT OFF>\n");}
|
||||
break;
|
||||
case TRACK_MODE_DC:
|
||||
if (pstate) {format=F("<= %c DC %d ON>\n");} else {format=F("<= %c DC %d OFF>\n");}
|
||||
break;
|
||||
case TRACK_MODE_DCX:
|
||||
if (pstate) {format=F("<= %c DCX %d ON>\n");} else {format=F("<= %c DCX %d OFF>\n");}
|
||||
break;
|
||||
default:
|
||||
break; // unknown, dont care
|
||||
}
|
||||
|
||||
if (stream) StringFormatter::send(stream,format,'A'+t, trackDCAddr[t]);
|
||||
else CommandDistributor::broadcastTrackState(format,'A'+t, trackDCAddr[t]);
|
||||
|
||||
}
|
||||
|
||||
@@ -454,13 +411,13 @@ void TrackManager::loop() {
|
||||
if (nextCycleTrack>lastTrack) nextCycleTrack=0;
|
||||
if (track[nextCycleTrack]==NULL) return;
|
||||
MotorDriver * motorDriver=track[nextCycleTrack];
|
||||
bool useProgLimit=dontLimitProg ? false : (bool)(track[nextCycleTrack]->getMode() & TRACK_MODE_PROG);
|
||||
bool useProgLimit=dontLimitProg? false: track[nextCycleTrack]->getMode()==TRACK_MODE_PROG;
|
||||
motorDriver->checkPowerOverload(useProgLimit, nextCycleTrack);
|
||||
}
|
||||
|
||||
MotorDriver * TrackManager::getProgDriver() {
|
||||
FOR_EACH_TRACK(t)
|
||||
if (track[t]->getMode() & TRACK_MODE_PROG) return track[t];
|
||||
if (track[t]->getMode()==TRACK_MODE_PROG) return track[t];
|
||||
return NULL;
|
||||
}
|
||||
|
||||
@@ -468,98 +425,77 @@ MotorDriver * TrackManager::getProgDriver() {
|
||||
std::vector<MotorDriver *>TrackManager::getMainDrivers() {
|
||||
std::vector<MotorDriver *> v;
|
||||
FOR_EACH_TRACK(t)
|
||||
if (track[t]->getMode() & TRACK_MODE_MAIN) v.push_back(track[t]);
|
||||
if (track[t]->getMode()==TRACK_MODE_MAIN) v.push_back(track[t]);
|
||||
return v;
|
||||
}
|
||||
#endif
|
||||
|
||||
// Set track power for all tracks with this mode
|
||||
void TrackManager::setTrackPower(TRACK_MODE trackmodeToMatch, POWERMODE powermode) {
|
||||
bool didChange=false;
|
||||
FOR_EACH_TRACK(t) {
|
||||
MotorDriver *driver=track[t];
|
||||
TRACK_MODE trackmodeOfTrack = driver->getMode();
|
||||
if (trackmodeToMatch & trackmodeOfTrack) {
|
||||
if (powermode != driver->getPower())
|
||||
didChange=true;
|
||||
if (powermode == POWERMODE::ON) {
|
||||
if (trackmodeOfTrack & TRACK_MODE_DC) {
|
||||
driver->setBrake(true); // DC starts with brake on
|
||||
applyDCSpeed(t); // speed match DCC throttles
|
||||
} else {
|
||||
// toggle brake before turning power on - resets overcurrent error
|
||||
// on the Pololu board if brake is wired to ^D2.
|
||||
driver->setBrake(true);
|
||||
driver->setBrake(false); // DCC runs with brake off
|
||||
}
|
||||
}
|
||||
driver->setPower(powermode);
|
||||
void TrackManager::setPower2(bool setProg,bool setJoin, POWERMODE mode) {
|
||||
if (!setProg) mainPowerGuess=mode;
|
||||
FOR_EACH_TRACK(t) {
|
||||
|
||||
TrackManager::setTrackPower(setProg, setJoin, mode, t);
|
||||
|
||||
}
|
||||
}
|
||||
if (didChange)
|
||||
CommandDistributor::broadcastPower();
|
||||
return;
|
||||
}
|
||||
|
||||
// Set track power for this track, inependent of mode
|
||||
void TrackManager::setTrackPower(POWERMODE powermode, byte t) {
|
||||
MotorDriver *driver=track[t];
|
||||
TRACK_MODE trackmode = driver->getMode();
|
||||
POWERMODE oldpower = driver->getPower();
|
||||
if (trackmode & TRACK_MODE_NONE) {
|
||||
driver->setBrake(true); // Track is unused. Brake is good to have.
|
||||
powermode = POWERMODE::OFF; // Track is unused. Force it to OFF
|
||||
} else if (trackmode & TRACK_MODE_DC) { // includes inverted DC (called DCX)
|
||||
if (powermode == POWERMODE::ON) {
|
||||
driver->setBrake(true); // DC starts with brake on
|
||||
applyDCSpeed(t); // speed match DCC throttles
|
||||
}
|
||||
} else /* MAIN PROG EXT BOOST */ {
|
||||
if (powermode == POWERMODE::ON) {
|
||||
// toggle brake before turning power on - resets overcurrent error
|
||||
// on the Pololu board if brake is wired to ^D2.
|
||||
driver->setBrake(true);
|
||||
driver->setBrake(false); // DCC runs with brake off
|
||||
}
|
||||
}
|
||||
driver->setPower(powermode);
|
||||
if (oldpower != driver->getPower())
|
||||
CommandDistributor::broadcastPower();
|
||||
void TrackManager::setTrackPower(bool setProg, bool setJoin, POWERMODE mode, byte thistrack) {
|
||||
|
||||
//DIAG(F("SetTrackPower Processing Track %d"), thistrack);
|
||||
MotorDriver * driver=track[thistrack];
|
||||
if (!driver) return;
|
||||
|
||||
switch (track[thistrack]->getMode()) {
|
||||
case TRACK_MODE_MAIN:
|
||||
if (setProg) break;
|
||||
// toggle brake before turning power on - resets overcurrent error
|
||||
// on the Pololu board if brake is wired to ^D2.
|
||||
// XXX see if we can make this conditional
|
||||
driver->setBrake(true);
|
||||
driver->setBrake(false); // DCC runs with brake off
|
||||
driver->setPower(mode);
|
||||
break;
|
||||
case TRACK_MODE_DC:
|
||||
case TRACK_MODE_DCX:
|
||||
//DIAG(F("Processing track - %d setProg %d"), thistrack, setProg);
|
||||
if (setProg || setJoin) break;
|
||||
driver->setBrake(true); // DC starts with brake on
|
||||
applyDCSpeed(thistrack); // speed match DCC throttles
|
||||
driver->setPower(mode);
|
||||
break;
|
||||
case TRACK_MODE_PROG:
|
||||
if (!setProg && !setJoin) break;
|
||||
driver->setBrake(true);
|
||||
driver->setBrake(false);
|
||||
driver->setPower(mode);
|
||||
break;
|
||||
case TRACK_MODE_EXT:
|
||||
driver->setBrake(true);
|
||||
driver->setBrake(false);
|
||||
driver->setPower(mode);
|
||||
break;
|
||||
case TRACK_MODE_NONE:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void TrackManager::reportPowerChange(Print* stream, byte thistrack) {
|
||||
// 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>
|
||||
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);
|
||||
}
|
||||
|
||||
// returns state of the one and only prog track
|
||||
POWERMODE TrackManager::getProgPower() {
|
||||
FOR_EACH_TRACK(t)
|
||||
if (track[t]->getMode() & TRACK_MODE_PROG)
|
||||
return track[t]->getPower(); // optimize: there is max one prog track
|
||||
return POWERMODE::OFF;
|
||||
}
|
||||
|
||||
// returns on if all are on. returns off otherwise
|
||||
POWERMODE TrackManager::getMainPower() {
|
||||
POWERMODE result = POWERMODE::OFF;
|
||||
FOR_EACH_TRACK(t) {
|
||||
if (track[t]->getMode() & TRACK_MODE_MAIN) {
|
||||
POWERMODE p = track[t]->getPower();
|
||||
if (p == POWERMODE::OFF)
|
||||
return POWERMODE::OFF; // done and out
|
||||
if (p == POWERMODE::ON)
|
||||
result = POWERMODE::ON;
|
||||
}
|
||||
FOR_EACH_TRACK(t)
|
||||
if (track[t]->getMode()==TRACK_MODE_PROG)
|
||||
return track[t]->getPower();
|
||||
return POWERMODE::OFF;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool TrackManager::getPower(byte t, char s[]) {
|
||||
if (t > lastTrack)
|
||||
return false;
|
||||
if (track[t]) {
|
||||
s[0] = track[t]->getPower() == POWERMODE::ON ? '1' : '0';
|
||||
s[2] = t + 'A';
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void TrackManager::reportObsoleteCurrent(Print* stream) {
|
||||
// This function is for backward JMRI compatibility only
|
||||
@@ -601,7 +537,7 @@ void TrackManager::setJoin(bool joined) {
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
if (joined) {
|
||||
FOR_EACH_TRACK(t) {
|
||||
if (track[t]->getMode() & TRACK_MODE_PROG) {
|
||||
if (track[t]->getMode()==TRACK_MODE_PROG) {
|
||||
tempProgTrack = t;
|
||||
setTrackMode(t, TRACK_MODE_MAIN);
|
||||
break;
|
||||
@@ -630,12 +566,12 @@ bool TrackManager::isPowerOn(byte t) {
|
||||
}
|
||||
|
||||
bool TrackManager::isProg(byte t) {
|
||||
if (track[t]->getMode() & TRACK_MODE_PROG)
|
||||
if (track[t]->getMode()==TRACK_MODE_PROG)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
TRACK_MODE TrackManager::getMode(byte t) {
|
||||
byte TrackManager::returnMode(byte t) {
|
||||
return (track[t]->getMode());
|
||||
}
|
||||
|
||||
@@ -643,3 +579,18 @@ int16_t TrackManager::returnDCAddr(byte t) {
|
||||
return (trackDCAddr[t]);
|
||||
}
|
||||
|
||||
const char* TrackManager::getModeName(byte Mode) {
|
||||
|
||||
//DIAG(F("PowerMode %d"), Mode);
|
||||
|
||||
switch (Mode)
|
||||
{
|
||||
case 1: return "NONE";
|
||||
case 2: return "MAIN";
|
||||
case 4: return "PROG";
|
||||
case 8: return "DC";
|
||||
case 16: return "DCX";
|
||||
case 32: return "EXT";
|
||||
default: return "----";
|
||||
}
|
||||
}
|
||||
|
@@ -62,39 +62,37 @@ class TrackManager {
|
||||
static void setDCSignal(int16_t cab, byte speedbyte);
|
||||
static MotorDriver * getProgDriver();
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
static std::vector<MotorDriver *>getMainDrivers();
|
||||
static std::vector<MotorDriver *>getMainDrivers();
|
||||
#endif
|
||||
|
||||
static void setPower2(bool progTrack,bool joinTrack,POWERMODE mode);
|
||||
static void setPower(POWERMODE mode) {setMainPower(mode); setProgPower(mode);}
|
||||
static void setTrackPower(POWERMODE mode, byte t);
|
||||
static void setTrackPower(TRACK_MODE trackmode, POWERMODE powermode);
|
||||
static void setMainPower(POWERMODE mode) {setTrackPower(TRACK_MODE_MAIN, mode);}
|
||||
static void setProgPower(POWERMODE mode) {setTrackPower(TRACK_MODE_PROG, mode);}
|
||||
static void setMainPower(POWERMODE mode) {setPower2(false,false,mode);}
|
||||
static void setProgPower(POWERMODE mode) {setPower2(true,false,mode);}
|
||||
static void setJoinPower(POWERMODE mode) {setPower2(false,true,mode);}
|
||||
static void setTrackPower(bool setProg, bool setJoin, POWERMODE mode, byte thistrack);
|
||||
|
||||
|
||||
static const int16_t MAX_TRACKS=8;
|
||||
static bool setTrackMode(byte track, TRACK_MODE mode, int16_t DCaddr=0);
|
||||
static bool parseEqualSign(Print * stream, int16_t params, int16_t p[]);
|
||||
static bool parseJ(Print * stream, int16_t params, int16_t p[]);
|
||||
static void loop();
|
||||
static POWERMODE getMainPower();
|
||||
static POWERMODE getMainPower() {return mainPowerGuess;}
|
||||
static POWERMODE getProgPower();
|
||||
static inline POWERMODE getPower(byte t) { return track[t]->getPower(); }
|
||||
static bool getPower(byte t, char s[]);
|
||||
static void setJoin(bool join);
|
||||
static bool isJoined() { return progTrackSyncMain;}
|
||||
static inline bool isActive (byte tr) {
|
||||
if (tr > lastTrack) return false;
|
||||
return track[tr]->getMode() & (TRACK_MODE_MAIN|TRACK_MODE_PROG|TRACK_MODE_DC|TRACK_MODE_BOOST|TRACK_MODE_EXT);}
|
||||
static void setJoinRelayPin(byte joinRelayPin);
|
||||
static void sampleCurrent();
|
||||
static void reportGauges(Print* stream);
|
||||
static void reportCurrent(Print* stream);
|
||||
static void reportPowerChange(Print* stream, byte thistrack);
|
||||
static void reportObsoleteCurrent(Print* stream);
|
||||
static void streamTrackState(Print* stream, byte t);
|
||||
static bool isPowerOn(byte t);
|
||||
static bool isProg(byte t);
|
||||
static TRACK_MODE getMode(byte t);
|
||||
static byte returnMode(byte t);
|
||||
static int16_t returnDCAddr(byte t);
|
||||
static const FSH* getModeName(TRACK_MODE Mode);
|
||||
static const char* getModeName(byte Mode);
|
||||
|
||||
static int16_t joinRelay;
|
||||
static bool progTrackSyncMain; // true when prog track is a siding switched to main
|
||||
@@ -111,9 +109,10 @@ class TrackManager {
|
||||
static void addTrack(byte t, MotorDriver* driver);
|
||||
static byte lastTrack;
|
||||
static byte nextCycleTrack;
|
||||
static POWERMODE mainPowerGuess;
|
||||
static void applyDCSpeed(byte t);
|
||||
|
||||
static int16_t trackDCAddr[MAX_TRACKS]; // dc address if TRACK_MODE_DC
|
||||
static int16_t trackDCAddr[MAX_TRACKS]; // dc address if TRACK_MODE_DC or TRACK_MODE_DCX
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
static byte tempProgTrack; // holds the prog track number during join
|
||||
#endif
|
||||
|
@@ -150,6 +150,7 @@ void WiThrottle::parse(RingStream * stream, byte * cmdx) {
|
||||
DCCWaveform::progTrack.setPowerMode(cmd[3]=='1'?POWERMODE::ON:POWERMODE::OFF);
|
||||
*/
|
||||
|
||||
CommandDistributor::broadcastPower();
|
||||
}
|
||||
#if defined(EXRAIL_ACTIVE)
|
||||
else if (cmd[1]=='R' && cmd[2]=='A' && cmd[3]=='2' ) { // Route activate
|
||||
@@ -495,6 +496,7 @@ void WiThrottle::getLocoCallback(int16_t locoid) {
|
||||
TrackManager::setJoin(true); // <1 JOIN> so we can drive loco away
|
||||
DIAG(F("LocoCallback commit success"));
|
||||
stashStream->commit();
|
||||
CommandDistributor::broadcastPower();
|
||||
}
|
||||
|
||||
void WiThrottle::sendIntro(Print* stream) {
|
||||
|
@@ -74,39 +74,25 @@ class NetworkClient {
|
||||
public:
|
||||
NetworkClient(WiFiClient c) {
|
||||
wifi = c;
|
||||
inUse = true;
|
||||
};
|
||||
bool active(byte clientId) {
|
||||
if (!inUse)
|
||||
return false;
|
||||
if(!wifi.connected()) {
|
||||
DIAG(F("Remove client %d"), clientId);
|
||||
CommandDistributor::forget(clientId);
|
||||
wifi.stop();
|
||||
inUse = false;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool ok() {
|
||||
return (inUse && wifi.connected());
|
||||
};
|
||||
bool recycle(WiFiClient c) {
|
||||
if (wifi == c) {
|
||||
if (inUse == true)
|
||||
DIAG(F("WARNING: Duplicate"));
|
||||
else
|
||||
DIAG(F("Returning"));
|
||||
inUse = true;
|
||||
return true;
|
||||
}
|
||||
if (inUse == false) {
|
||||
wifi = c;
|
||||
inUse = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (inUse == true) return false;
|
||||
|
||||
// return false here until we have
|
||||
// implemented a LRU timer
|
||||
// if (LRU too recent) return false;
|
||||
return false;
|
||||
|
||||
wifi = c;
|
||||
inUse = true;
|
||||
return true;
|
||||
};
|
||||
WiFiClient wifi;
|
||||
private:
|
||||
bool inUse;
|
||||
bool inUse = true;
|
||||
};
|
||||
|
||||
static std::vector<NetworkClient> clients; // a list to hold all clients
|
||||
@@ -177,9 +163,7 @@ bool WifiESP::setup(const char *SSid,
|
||||
delay(500);
|
||||
}
|
||||
if (WiFi.status() == WL_CONNECTED) {
|
||||
// DIAG(F("Wifi STA IP %s"),WiFi.localIP().toString().c_str());
|
||||
DIAG(F("Wifi in STA mode"));
|
||||
LCD(7, F("IP: %s"), WiFi.localIP().toString().c_str());
|
||||
DIAG(F("Wifi STA IP %s"),WiFi.localIP().toString().c_str());
|
||||
wifiUp = true;
|
||||
} else {
|
||||
DIAG(F("Could not connect to Wifi SSID %s"),SSid);
|
||||
@@ -225,12 +209,8 @@ bool WifiESP::setup(const char *SSid,
|
||||
if (WiFi.softAP(strSSID.c_str(),
|
||||
havePassword ? password : strPass.c_str(),
|
||||
channel, false, 8)) {
|
||||
// DIAG(F("Wifi AP SSID %s PASS %s"),strSSID.c_str(),havePassword ? password : strPass.c_str());
|
||||
DIAG(F("Wifi in AP mode"));
|
||||
LCD(5, F("Wifi: %s"), strSSID.c_str());
|
||||
LCD(6, F("PASS: %s"),havePassword ? password : strPass.c_str());
|
||||
// DIAG(F("Wifi AP IP %s"),WiFi.softAPIP().toString().c_str());
|
||||
LCD(7, F("IP: %s"),WiFi.softAPIP().toString().c_str());
|
||||
DIAG(F("Wifi AP SSID %s PASS %s"),strSSID.c_str(),havePassword ? password : strPass.c_str());
|
||||
DIAG(F("Wifi AP IP %s"),WiFi.softAPIP().toString().c_str());
|
||||
wifiUp = true;
|
||||
APmode = true;
|
||||
} else {
|
||||
@@ -296,26 +276,37 @@ void WifiESP::loop() {
|
||||
// really no good way to check for LISTEN especially in AP mode?
|
||||
wl_status_t wlStatus;
|
||||
if (APmode || (wlStatus = WiFi.status()) == WL_CONNECTED) {
|
||||
// loop over all clients and remove inactive
|
||||
for (clientId=0; clientId<clients.size(); clientId++){
|
||||
// check if client is there and alive
|
||||
if(clients[clientId].inUse && !clients[clientId].wifi.connected()) {
|
||||
DIAG(F("Remove client %d"), clientId);
|
||||
CommandDistributor::forget(clientId);
|
||||
clients[clientId].wifi.stop();
|
||||
clients[clientId].inUse = false;
|
||||
//Do NOT clients.erase(clients.begin()+clientId) as
|
||||
//that would mix up clientIds for later.
|
||||
}
|
||||
}
|
||||
if (server->hasClient()) {
|
||||
WiFiClient client;
|
||||
while (client = server->available()) {
|
||||
for (clientId=0; clientId<clients.size(); clientId++){
|
||||
if (clients[clientId].recycle(client)) {
|
||||
DIAG(F("Recycle client %d %s:%d"), clientId, client.remoteIP().toString().c_str(),client.remotePort());
|
||||
DIAG(F("Recycle client %d %s"), clientId, client.remoteIP().toString().c_str());
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (clientId>=clients.size()) {
|
||||
NetworkClient nc(client);
|
||||
clients.push_back(nc);
|
||||
DIAG(F("New client %d, %s:%d"), clientId, client.remoteIP().toString().c_str(),client.remotePort());
|
||||
DIAG(F("New client %d, %s"), clientId, client.remoteIP().toString().c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
// loop over all connected clients
|
||||
// this removes as a side effect inactive clients when checking ::active()
|
||||
for (clientId=0; clientId<clients.size(); clientId++){
|
||||
if(clients[clientId].active(clientId)) {
|
||||
if(clients[clientId].ok()) {
|
||||
int len;
|
||||
if ((len = clients[clientId].wifi.available()) > 0) {
|
||||
// read data from client
|
||||
@@ -353,7 +344,7 @@ void WifiESP::loop() {
|
||||
}
|
||||
// buffer filled, end with '\0' so we can use it as C string
|
||||
buffer[count]='\0';
|
||||
if((unsigned int)clientId <= clients.size() && clients[clientId].active(clientId)) {
|
||||
if((unsigned int)clientId <= clients.size() && clients[clientId].ok()) {
|
||||
if (Diag::CMD || Diag::WITHROTTLE)
|
||||
DIAG(F("SEND %d:%s"), clientId, buffer);
|
||||
clients[clientId].wifi.write(buffer,count);
|
||||
@@ -386,9 +377,8 @@ void WifiESP::loop() {
|
||||
// prio task. On core1 this is not a problem
|
||||
// as there the wdt is disabled by the
|
||||
// arduio IDE startup routines.
|
||||
if (xPortGetCoreID() == 0) {
|
||||
if (xPortGetCoreID() == 0)
|
||||
feedTheDog0();
|
||||
yield();
|
||||
}
|
||||
yield();
|
||||
}
|
||||
#endif //ESP32
|
||||
|
@@ -3,6 +3,7 @@
|
||||
* © 2020-2022 Harald Barth
|
||||
* © 2020-2022 Chris Harlow
|
||||
* © 2023 Nathan Kellenicki
|
||||
* © 2023 Travis Farmer
|
||||
* All rights reserved.
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
@@ -57,6 +58,14 @@ Stream * WifiInterface::wifiStream;
|
||||
#define SERIAL3 Serial3
|
||||
#endif
|
||||
|
||||
#if defined(ARDUINO_GIGA) // yes giga
|
||||
#define NUM_SERIAL 5
|
||||
#define SERIAL1 Serial1
|
||||
#define SERIAL2 Serial2
|
||||
#define SERIAL3 Serial3
|
||||
#define SERIAL4 Serial4
|
||||
#endif // giga
|
||||
|
||||
#if defined(ARDUINO_ARCH_STM32)
|
||||
// Handle serial ports availability on STM32 for variants!
|
||||
// #undef NUM_SERIAL
|
||||
@@ -68,9 +77,7 @@ Stream * WifiInterface::wifiStream;
|
||||
#define NUM_SERIAL 3
|
||||
#define SERIAL1 Serial3
|
||||
#define SERIAL3 Serial5
|
||||
#elif defined(ARDUINO_NUCLEO_F413ZH) || defined(ARDUINO_NUCLEO_F429ZI) \
|
||||
|| defined(ARDUINO_NUCLEO_F446ZE) || defined(ARDUINO_NUCLEO_F412ZG) \
|
||||
|| defined(ARDUINO_NUCLEO_F439ZI)
|
||||
#elif defined(ARDUINO_NUCLEO_F413ZH) || defined(ARDUINO_NUCLEO_F429ZI) || defined(ARDUINO_NUCLEO_F446ZE) || defined(ARDUINO_NUCLEO_F412ZG)
|
||||
#define NUM_SERIAL 2
|
||||
#define SERIAL1 Serial6
|
||||
#else
|
||||
@@ -203,16 +210,15 @@ wifiSerialState WifiInterface::setup2(const FSH* SSid, const FSH* password,
|
||||
// Display the AT version information
|
||||
StringFormatter::send(wifiStream, F("AT+GMR\r\n"));
|
||||
if (checkForOK(2000, F("AT version:"), true, false)) {
|
||||
char version[] = "0.0.0.0-xxx";
|
||||
for (int i=0; i<11;i++) {
|
||||
char version[] = "0.0.0.0";
|
||||
for (int i=0; i<8;i++) {
|
||||
while(!wifiStream->available());
|
||||
version[i]=wifiStream->read();
|
||||
StringFormatter::printEscape(version[i]);
|
||||
}
|
||||
if ((version[0] == '0') ||
|
||||
(version[0] == '2' && version[2] == '0') ||
|
||||
(version[0] == '2' && version[2] == '2' && version[4] == '0' && version[6] == '0'
|
||||
&& version[7] == '-' && version[8] == 'd' && version[9] == 'e' && version[10] == 'v')) {
|
||||
(version[0] == '2' && version[2] == '2' && version[4] == '0' && version[6] == '0')) {
|
||||
DIAG(F("You need to up/downgrade the ESP firmware"));
|
||||
SSid = F("UPDATE_ESP_FIRMWARE");
|
||||
forceAP = true;
|
||||
|
445
Wifi_NINA.cpp
Normal file
445
Wifi_NINA.cpp
Normal file
@@ -0,0 +1,445 @@
|
||||
/*
|
||||
© 2023 Paul M. Antoine
|
||||
© 2021 Harald Barth
|
||||
© 2023 Nathan Kellenicki
|
||||
|
||||
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/>.
|
||||
*/
|
||||
#include "defines.h"
|
||||
|
||||
#ifdef WIFI_NINA
|
||||
//#include <vector>
|
||||
#include <SPI.h>
|
||||
#ifndef ARDUINO_GIGA
|
||||
#include <WifiNINA.h>
|
||||
#else
|
||||
#include <WiFi.h>
|
||||
#endif
|
||||
#include "Wifi_NINA.h"
|
||||
// #include "ESPmDNS.h"
|
||||
// #include <WiFi.h>
|
||||
// #include "esp_wifi.h"
|
||||
// #include "WifiESP32.h"
|
||||
// #include <SPI.h>
|
||||
#include "DIAG.h"
|
||||
#include "RingStream.h"
|
||||
#include "CommandDistributor.h"
|
||||
#include "WiThrottle.h"
|
||||
|
||||
// Configure the pins used for the ESP32 connection
|
||||
#if !defined(ARDUINO_GIGA) && defined(ARDUINO_ARCH_STM32) // Here my STM32 configuration
|
||||
#define SPIWIFI SPI // The SPI port
|
||||
#define SPIWIFI_SS PA4 // Chip select pin
|
||||
#define ESP32_RESETN PA10 // Reset pin
|
||||
#define SPIWIFI_ACK PB3 // a.k.a BUSY or READY pin
|
||||
#define ESP32_GPIO0 -1
|
||||
#else
|
||||
#warning "WiFiNINA has no SPI port or pin allocations for this archiecture yet!"
|
||||
#endif
|
||||
#define MAX_CLIENTS 4
|
||||
/*class NetworkClient {
|
||||
public:
|
||||
NetworkClient(WiFiClient c) {
|
||||
wifi = c;
|
||||
};
|
||||
bool ok() {
|
||||
return (inUse && wifi.connected());
|
||||
};
|
||||
bool recycle(WiFiClient c) {
|
||||
|
||||
if (inUse == true) return false;
|
||||
|
||||
// return false here until we have
|
||||
// implemented a LRU timer
|
||||
// if (LRU too recent) return false;
|
||||
//return false;
|
||||
|
||||
wifi = c;
|
||||
inUse = true;
|
||||
return true;
|
||||
};
|
||||
WiFiClient wifi;
|
||||
bool inUse = true;
|
||||
};*/
|
||||
|
||||
//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;
|
||||
static IPAddress ip;
|
||||
|
||||
// #ifdef WIFI_TASK_ON_CORE0
|
||||
// void wifiLoop(void *){
|
||||
// for(;;){
|
||||
// WifiNINA::loop();
|
||||
// }
|
||||
// }
|
||||
// #endif
|
||||
|
||||
char asciitolower(char in) {
|
||||
if (in <= 'Z' && in >= 'A')
|
||||
return in - ('Z' - 'z');
|
||||
return in;
|
||||
}
|
||||
|
||||
bool WifiNINA::setup(const char *SSid,
|
||||
const char *password,
|
||||
const char *hostname,
|
||||
int port,
|
||||
const byte channel,
|
||||
const bool forceAP) {
|
||||
bool havePassword = true;
|
||||
bool haveSSID = true;
|
||||
bool wifiUp = false;
|
||||
uint8_t tries = 40;
|
||||
|
||||
// Set up the pins!
|
||||
#ifndef ARDUINO_GIGA
|
||||
WiFi.setPins(SPIWIFI_SS, SPIWIFI_ACK, ESP32_RESETN, ESP32_GPIO0, &SPIWIFI);
|
||||
#endif
|
||||
// check for the WiFi module:
|
||||
if (WiFi.status() == WL_NO_MODULE) {
|
||||
DIAG(F("Communication with WiFi module failed!"));
|
||||
// don't continue for now!
|
||||
while (true);
|
||||
}
|
||||
|
||||
// Print firmware version on the module
|
||||
String fv = WiFi.firmwareVersion();
|
||||
DIAG(F("WifiNINA Firmware version found:%s"), fv.c_str());
|
||||
|
||||
// clean start
|
||||
// WiFi.mode(WIFI_STA);
|
||||
// WiFi.disconnect(true);
|
||||
// differnet settings that did not improve for haba
|
||||
// WiFi.useStaticBuffers(true);
|
||||
// WiFi.setScanMethod(WIFI_ALL_CHANNEL_SCAN);
|
||||
// WiFi.setSortMethod(WIFI_CONNECT_AP_BY_SECURITY);
|
||||
|
||||
const char *yourNetwork = "Your network ";
|
||||
if (strncmp(yourNetwork, SSid, 13) == 0 || strncmp("", SSid, 13) == 0)
|
||||
haveSSID = false;
|
||||
if (strncmp(yourNetwork, password, 13) == 0 || strncmp("", password, 13) == 0)
|
||||
havePassword = false;
|
||||
|
||||
if (haveSSID && havePassword && !forceAP) {
|
||||
#ifndef ARDUINO_GIGA
|
||||
WiFi.setHostname(hostname); // Strangely does not work unless we do it HERE!
|
||||
#endif
|
||||
// WiFi.mode(WIFI_STA);
|
||||
// WiFi.setAutoReconnect(true);
|
||||
WiFi.begin(SSid, password);
|
||||
while (WiFi.status() != WL_CONNECTED && tries) {
|
||||
Serial.print('.');
|
||||
tries--;
|
||||
delay(500);
|
||||
}
|
||||
if (WiFi.status() == WL_CONNECTED) {
|
||||
// String ip_str = sprintf("%xl", WiFi.localIP());
|
||||
DIAG(F("Wifi STA IP %d.%d.%d.%d"), WiFi.localIP()[0], WiFi.localIP()[1],WiFi.localIP()[2],WiFi.localIP()[3]);
|
||||
wifiUp = true;
|
||||
} else {
|
||||
DIAG(F("Could not connect to Wifi SSID %s"),SSid);
|
||||
DIAG(F("Forcing one more Wifi restart"));
|
||||
// esp_wifi_start();
|
||||
// esp_wifi_connect();
|
||||
tries=40;
|
||||
while (WiFi.status() != WL_CONNECTED && tries) {
|
||||
Serial.print('.');
|
||||
tries--;
|
||||
delay(500);
|
||||
}
|
||||
if (WiFi.status() == WL_CONNECTED) {
|
||||
ip = WiFi.localIP();
|
||||
DIAG(F("Wifi STA IP 2nd try %d.%d.%d.%d"), ip[0], ip[1], ip[2], ip[3]);
|
||||
wifiUp = true;
|
||||
} else {
|
||||
DIAG(F("Wifi STA mode FAIL. Will revert to AP mode"));
|
||||
haveSSID=false;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!haveSSID || forceAP) {
|
||||
// prepare all strings
|
||||
String strSSID(forceAP ? SSid : "DCCEX_");
|
||||
String strPass(forceAP ? password : "PASS_");
|
||||
if (!forceAP) {
|
||||
byte mac[6];
|
||||
WiFi.macAddress(mac);
|
||||
String strMac;
|
||||
for (int i = 0; i++; i < 6) {
|
||||
strMac += String(mac[i], HEX);
|
||||
}
|
||||
|
||||
DIAG(F("MAC address: %x:%x:%x:%x:%x:%x"), mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
|
||||
|
||||
strMac.remove(0,9);
|
||||
strMac.replace(":","");
|
||||
strMac.replace(":","");
|
||||
// convert mac addr hex chars to lower case to be compatible with AT software
|
||||
std::transform(strMac.begin(), strMac.end(), strMac.begin(), asciitolower);
|
||||
strSSID.concat(strMac);
|
||||
strPass.concat(strMac);
|
||||
}
|
||||
|
||||
if (WiFi.beginAP(strSSID.c_str(),
|
||||
havePassword ? password : strPass.c_str(),
|
||||
channel) == WL_AP_LISTENING) {
|
||||
DIAG(F("Wifi AP SSID %s PASS %s"),strSSID.c_str(),havePassword ? password : strPass.c_str());
|
||||
ip = WiFi.localIP();
|
||||
DIAG(F("Wifi AP IP %d.%d.%d.%d"),ip[0], ip[1], ip[2], ip[3]);
|
||||
wifiUp = true;
|
||||
APmode = true;
|
||||
} else {
|
||||
DIAG(F("Could not set up AP with Wifi SSID %s"),strSSID.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (!wifiUp) {
|
||||
DIAG(F("Wifi setup all fail (STA and AP mode)"));
|
||||
// no idea to go on
|
||||
return false;
|
||||
}
|
||||
|
||||
// TODO: we need to run the MDNS_Generic server I suspect
|
||||
// // 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)) {
|
||||
// DIAG(F("Wifi setup failed to add withrottle service to mDNS"));
|
||||
// }
|
||||
|
||||
server = new WiFiServer(port); // start listening on tcp port
|
||||
server->begin();
|
||||
// server started here
|
||||
|
||||
// #ifdef WIFI_TASK_ON_CORE0
|
||||
// //start loop task
|
||||
// if (pdPASS != xTaskCreatePinnedToCore(
|
||||
// wifiLoop, /* Task function. */
|
||||
// "wifiLoop",/* name of task. */
|
||||
// 10000, /* Stack size of task */
|
||||
// NULL, /* parameter of the task */
|
||||
// 1, /* priority of the task */
|
||||
// NULL, /* Task handle to keep track of created task */
|
||||
// 0)) { /* pin task to core 0 */
|
||||
// DIAG(F("Could not create wifiLoop task"));
|
||||
// return false;
|
||||
// }
|
||||
|
||||
// // report server started after wifiLoop creation
|
||||
// // when everything looks good
|
||||
// DIAG(F("Server starting (core 0) port %d"),port);
|
||||
// #else
|
||||
DIAG(F("Server will be started on port %d"),port);
|
||||
// #endif
|
||||
ip = WiFi.localIP();
|
||||
LCD(4,F("IP: %d.%d.%d.%d"), ip[0], ip[1], ip[2], ip[3]);
|
||||
LCD(5,F("Port:%d"), port);
|
||||
return true;
|
||||
}
|
||||
|
||||
const char *wlerror[] = {
|
||||
"WL_IDLE_STATUS",
|
||||
"WL_NO_SSID_AVAIL",
|
||||
"WL_SCAN_COMPLETED",
|
||||
"WL_CONNECTED",
|
||||
"WL_CONNECT_FAILED",
|
||||
"WL_CONNECTION_LOST",
|
||||
"WL_DISCONNECTED"
|
||||
};
|
||||
|
||||
/*void WifiNINA::loop() {
|
||||
int clientId; //tmp loop var
|
||||
|
||||
// really no good way to check for LISTEN especially in AP mode?
|
||||
wl_status_t wlStatus;
|
||||
if (APmode || (wlStatus = (wl_status_t)WiFi.status()) == WL_CONNECTED) {
|
||||
// loop over all clients and remove inactive
|
||||
for (clientId=0; clientId<clients.size(); clientId++){
|
||||
// check if client is there and alive
|
||||
if(clients[clientId].inUse && !clients[clientId].wifi.connected()) {
|
||||
DIAG(F("Remove client %d"), clientId);
|
||||
CommandDistributor::forget(clientId);
|
||||
clients[clientId].wifi.stop();
|
||||
clients[clientId].inUse = false;
|
||||
|
||||
//Do NOT clients.erase(clients.begin()+clientId) as
|
||||
//that would mix up clientIds for later.
|
||||
}
|
||||
}
|
||||
WiFiClient client = server->available();
|
||||
if (client) {
|
||||
///while (client.available() == true) {
|
||||
for (clientId=0; clientId<clients.size(); clientId++){
|
||||
if (clients[clientId].recycle(client)) {
|
||||
ip = client.remoteIP();
|
||||
DIAG(F("Recycle client %d %d.%d.%d.%d"), clientId, ip[0], ip[1], ip[2], ip[3]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (clientId>=clients.size()) {
|
||||
NetworkClient* nc=new NetworkClient(client);
|
||||
clients.push_back(*nc);
|
||||
//delete nc;
|
||||
ip = client.remoteIP();
|
||||
DIAG(F("New client %d, %d.%d.%d.%d"), clientId, ip[0], ip[1], ip[2], ip[3]);
|
||||
}
|
||||
///}
|
||||
}
|
||||
// loop over all connected clients
|
||||
for (clientId=0; clientId<clients.size(); clientId++){
|
||||
if(clients[clientId].ok()) {
|
||||
int len;
|
||||
if ((len = clients[clientId].wifi.available()) > 0) {
|
||||
// read data from client
|
||||
byte cmd[len+1];
|
||||
for(int i=0; i<len; i++) {
|
||||
cmd[i]=clients[clientId].wifi.read();
|
||||
}
|
||||
cmd[len]=0;
|
||||
CommandDistributor::parse(clientId,cmd,outboundRing);
|
||||
}
|
||||
}
|
||||
} // all clients
|
||||
|
||||
WiThrottle::loop(outboundRing);
|
||||
|
||||
// something to write out?
|
||||
clientId=outboundRing->read();
|
||||
if (clientId >= 0) {
|
||||
// We have data to send in outboundRing
|
||||
// and we have a valid clientId.
|
||||
// First read it out to buffer
|
||||
// and then look if it can be sent because
|
||||
// we can not leave it in the ring for ever
|
||||
int count=outboundRing->count();
|
||||
{
|
||||
char buffer[count+1]; // one extra for '\0'
|
||||
for(int i=0;i<count;i++) {
|
||||
int c = outboundRing->read();
|
||||
if (c >= 0) // Panic check, should never be false
|
||||
buffer[i] = (char)c;
|
||||
else {
|
||||
DIAG(F("Ringread fail at %d"),i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
// buffer filled, end with '\0' so we can use it as C string
|
||||
buffer[count]='\0';
|
||||
if((unsigned int)clientId <= clients.size() && clients[clientId].ok()) {
|
||||
if (Diag::CMD || Diag::WITHROTTLE)
|
||||
DIAG(F("SEND %d:%s"), clientId, buffer);
|
||||
clients[clientId].wifi.write(buffer,count);
|
||||
} else {
|
||||
DIAG(F("Unsent(%d): %s"), clientId, buffer);
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (!APmode) { // in STA mode but not connected any more
|
||||
// kick it again
|
||||
if (wlStatus <= 6) {
|
||||
DIAG(F("Wifi aborted with error %s. Kicking Wifi!"), wlerror[wlStatus]);
|
||||
// esp_wifi_start();
|
||||
// esp_wifi_connect();
|
||||
uint8_t tries=40;
|
||||
while (WiFi.status() != WL_CONNECTED && tries) {
|
||||
Serial.print('.');
|
||||
tries--;
|
||||
delay(500);
|
||||
}
|
||||
} else {
|
||||
// all well, probably
|
||||
//DIAG(F("Running BT"));
|
||||
}
|
||||
}
|
||||
}*/
|
||||
|
||||
WiFiClient clients[MAX_CLIENTS]; // nulled in setup
|
||||
|
||||
void WifiNINA::checkForNewClient() {
|
||||
auto newClient=server->available();
|
||||
if (!newClient) return;
|
||||
for (byte clientId=0; clientId<MAX_CLIENTS; clientId++){
|
||||
if (!clients[clientId]) {
|
||||
clients[clientId]=newClient; // use this slot
|
||||
DIAG(F("New client connected to slot %d"),clientId); //TJF: brought in for debugging.
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void WifiNINA::checkForLostClients() {
|
||||
for (byte clientId=0; clientId<MAX_CLIENTS; clientId++){
|
||||
auto c=clients[clientId];
|
||||
if(c && !c.connected()) {
|
||||
DIAG(F("Remove client %d"), clientId);
|
||||
CommandDistributor::forget(clientId);
|
||||
//delete c; //TJF: this causes a crash when client drops.. commenting out for now.
|
||||
//clients[clientId]=NULL; // TJF: what to do... what to do...
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void WifiNINA::checkForClientInput() {
|
||||
// Find a client providing input
|
||||
for (byte clientId=0; clientId<MAX_CLIENTS; clientId++){
|
||||
auto c=clients[clientId];
|
||||
if(c) {
|
||||
auto len=c.available();
|
||||
if (len) {
|
||||
// read data from client
|
||||
byte cmd[len+1];
|
||||
for(int i=0; i<len; i++) cmd[i]=c.read();
|
||||
cmd[len]=0;
|
||||
CommandDistributor::parse(clientId,cmd,outboundRing);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void WifiNINA::checkForClientOutput() {
|
||||
// something to write out?
|
||||
auto clientId=outboundRing->read();
|
||||
if (clientId < 0) return;
|
||||
auto replySize=outboundRing->count();
|
||||
if (replySize==0) return; // nothing to send
|
||||
auto c=clients[clientId];
|
||||
if (!c) {
|
||||
// client is gone, throw away msg
|
||||
for (int i=0;i<replySize;i++) outboundRing->read();
|
||||
DIAG(F("gone, drop message.")); //TJF: only for diag
|
||||
return;
|
||||
}
|
||||
// emit data to the client object
|
||||
// This should work in theory, the
|
||||
DIAG(F("send message")); //TJF: only for diag
|
||||
//TJF: the old code had to add a 0x00 byte to the end to terminate the
|
||||
//TJF: c string, before sending it. i take it this is not needed?
|
||||
for (int i=0;i<replySize;i++) c.write(outboundRing->read());
|
||||
}
|
||||
|
||||
void WifiNINA::loop() {
|
||||
checkForLostClients(); // ***
|
||||
checkForNewClient();
|
||||
checkForClientInput(); // ***
|
||||
WiThrottle::loop(outboundRing); // allow withrottle to broadcast if needed
|
||||
checkForClientOutput();
|
||||
}
|
||||
|
||||
#endif // WIFI_NINA
|
46
Wifi_NINA.h
Normal file
46
Wifi_NINA.h
Normal file
@@ -0,0 +1,46 @@
|
||||
/*
|
||||
* © 2023 Paul M. Antoine
|
||||
* © 2021 Harald Barth
|
||||
* © 2023 Nathan Kellenicki
|
||||
*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#ifndef WifiNINA_h
|
||||
#define WifiNINA_h
|
||||
// #include "FSH.h"
|
||||
#include <Arduino.h>
|
||||
// #include <SPI.h>
|
||||
// #include <WifiNINA.h>
|
||||
|
||||
class WifiNINA
|
||||
{
|
||||
|
||||
public:
|
||||
static bool setup(const char *wifiESSID,
|
||||
const char *wifiPassword,
|
||||
const char *hostname,
|
||||
const int port,
|
||||
const byte channel,
|
||||
const bool forceAP);
|
||||
static void loop();
|
||||
private:
|
||||
static void checkForNewClient();
|
||||
static void checkForLostClients();
|
||||
static void checkForClientInput();
|
||||
static void checkForClientOutput();
|
||||
};
|
||||
#endif //WifiNINA_h
|
@@ -167,14 +167,6 @@ The configuration file for DCC-EX Command Station
|
||||
// * #define SCROLLMODE 2 is by row (move up 1 row at a time).
|
||||
#define SCROLLMODE 1
|
||||
|
||||
// In order to avoid wasting memory the current scroll buffer is limited
|
||||
// to 8 lines. Some users wishing to display additional information
|
||||
// such as TrackManager power states have requested additional rows aware
|
||||
// of the warning that this will take extra RAM. if you wish to include additional rows
|
||||
// uncomment the following #define and set the number of lines you need.
|
||||
//#define MAX_CHARACTER_ROWS 12
|
||||
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////
|
||||
// DISABLE EEPROM
|
||||
//
|
||||
@@ -199,18 +191,6 @@ The configuration file for DCC-EX Command Station
|
||||
//
|
||||
// #define DISABLE_PROG
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////
|
||||
// DISABLE / ENABLE VDPY
|
||||
//
|
||||
// The Virtual display "VDPY" feature is by default enabled everywhere
|
||||
// but on Uno and Nano. If you think you can fit it (for example
|
||||
// having disabled some of the features above) you can enable it with
|
||||
// ENABLE_VDPY. You can even disable it on all other CPUs with
|
||||
// DISABLE_VDPY
|
||||
//
|
||||
// #define DISABLE_VDPY
|
||||
// #define ENABLE_VDPY
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////
|
||||
// REDEFINE WHERE SHORT/LONG ADDR break is. According to NMRA the last short address
|
||||
// is 127 and the first long address is 128. There are manufacturers which have
|
||||
@@ -222,14 +202,6 @@ The configuration file for DCC-EX Command Station
|
||||
// We do not support to use the same address, for example 100(long) and 100(short)
|
||||
// at the same time, there must be a border.
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////
|
||||
// Some newer 32bit microcontrollers boot very quickly, so powering on I2C and other
|
||||
// peripheral devices at the same time may result in the CommandStation booting too
|
||||
// quickly to detect them.
|
||||
// To work around this, uncomment the STARTUP_DELAY line below and set a value in
|
||||
// milliseconds that works for your environment, default is 3000 (3 seconds).
|
||||
// #define STARTUP_DELAY 3000
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// DEFINE TURNOUTS/ACCESSORIES FOLLOW NORM RCN-213
|
||||
@@ -294,12 +266,6 @@ The configuration file for DCC-EX Command Station
|
||||
//
|
||||
//#define SERIAL_BT_COMMANDS
|
||||
|
||||
// BOOSTER PIN INPUT ON ESP32
|
||||
// On ESP32 you have the possibility to define a pin as booster input
|
||||
// Arduio pin D2 is GPIO 26 on ESPDuino32
|
||||
//
|
||||
//#define BOOSTER_INPUT 26
|
||||
|
||||
// SABERTOOTH
|
||||
//
|
||||
// This is a very special option and only useful if you happen to have a
|
||||
|
33
defines.h
33
defines.h
@@ -5,6 +5,7 @@
|
||||
* © 2021 Fred Decker
|
||||
* © 2020-2022 Harald Barth
|
||||
* © 2020-2021 Chris Harlow
|
||||
* © 2023 Travis Farmer
|
||||
*
|
||||
* This file is part of CommandStation-EX
|
||||
*
|
||||
@@ -147,7 +148,28 @@
|
||||
// #ifndef I2C_USE_WIRE
|
||||
// #define I2C_USE_WIRE
|
||||
// #endif
|
||||
|
||||
#elif defined(ARDUINO_GIGA)
|
||||
#define ARDUINO_TYPE "Giga"
|
||||
#ifndef GIGA_EXT_EEPROM
|
||||
#define DISABLE_EEPROM
|
||||
#endif
|
||||
#if defined(ENABLE_WIFI) && !defined(WIFI_NINA)
|
||||
#define WIFI_NINA
|
||||
#endif
|
||||
//#if !defined(I2C_USE_WIRE)
|
||||
//#define I2C_USE_WIRE
|
||||
//#endif
|
||||
#define SDA I2C_SDA
|
||||
#define SCL I2C_SCL
|
||||
#define DCC_EX_TIMER
|
||||
// these don't work...
|
||||
//extern const uint16_t PROGMEM port_to_input_PGM[];
|
||||
//extern const uint16_t PROGMEM port_to_output_PGM[];
|
||||
//extern const uint8_t PROGMEM digital_pin_to_bit_mask_PGM[];
|
||||
//#define digitalPinToBitMask(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM + (P) ) )
|
||||
//#define portOutputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_output_PGM + (P))) )
|
||||
//#define portInputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_input_PGM + (P))) )
|
||||
|
||||
/* TODO when ready
|
||||
#elif defined(ARDUINO_ARCH_RP2040)
|
||||
#define ARDUINO_TYPE "RP2040"
|
||||
@@ -219,10 +241,11 @@
|
||||
// The HAL is disabled by default on Nano and Uno platforms, because of limited flash space.
|
||||
//
|
||||
#if defined(ARDUINO_AVR_NANO) || defined(ARDUINO_AVR_UNO)
|
||||
#define IO_NO_HAL // HAL too big whatever you disable otherwise
|
||||
#ifndef ENABLE_VDPY
|
||||
#define DISABLE_VDPY
|
||||
#endif
|
||||
#if defined(DISABLE_DIAG) && defined(DISABLE_EEPROM) && defined(DISABLE_PROG)
|
||||
#warning you have sacrificed DIAG for HAL
|
||||
#else
|
||||
#define IO_NO_HAL
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if __has_include ( "myAutomation.h")
|
||||
|
49
version.h
49
version.h
@@ -3,53 +3,8 @@
|
||||
|
||||
#include "StringFormatter.h"
|
||||
|
||||
#define VERSION "5.2.27"
|
||||
// 5.2.27 - Bugfix: IOExpander memory allocation
|
||||
// 5.2.26 - Silently ignore overridden HAL defaults
|
||||
// - include HAL_IGNORE_DEFAULTS macro in EXRAIL
|
||||
// 5.2.25 - Fix bug causing <X> after working <D commands
|
||||
// 5.2.24 - Exrail macro asserts to catch
|
||||
// : duplicate/missing automation/route/sequence/call ids
|
||||
// : latches and reserves out of range
|
||||
// : speeds out of range
|
||||
// 5.2.23 - KeywordHasher _hk (no functional change)
|
||||
// 5.2.22 - Bugfixes: Empty turnout descriptions ok; negative route numbers valid.
|
||||
// 5.2.21 - Add STARTUP_DELAY config option to delay CS bootup
|
||||
// 5.2.20 - Check return of Ethernet.begin()
|
||||
// 5.2.19 - ESP32: Determine if the RMT hardware can handle DCC
|
||||
// 5.2.18 - Display network IP fix
|
||||
// 5.2.17 - ESP32 simplify network logic
|
||||
// 5.2.16 - Bugfix to allow for devices using the EX-IOExpander protocol to have no analogue or no digital pins
|
||||
// 5.2.15 - move call to CommandDistributor::broadcastPower() into the TrackManager::setTrackPower(*) functions
|
||||
// - add repeats to function packets that are not reminded in accordance with accessory packets
|
||||
// 5.2.14 - Reminder window DCC packet optimization
|
||||
// - Optional #define DISABLE_FUNCTION_REMINDERS
|
||||
// 5.2.13 - EXRAIL STEALTH
|
||||
// 5.2.12 - ESP32 add AP mode LCD messages with SSID/PW for
|
||||
// - STM32 change to UID_BASE constants in DCCTimerSTM32 rather than raw hex addresses for UID registers
|
||||
// - STM32 extra UART/USARTs for larger Nucleo models
|
||||
// 5.2.11 - Change from TrackManager::returnMode to TrackManager::getMode
|
||||
// 5.2.10 - Include trainbrains.eu block unoccupancy driver
|
||||
// - include IO_PCA9555
|
||||
// 5.2.9 - Bugfix LCD startup with no LCD, uses <@
|
||||
// 5.2.9 - EXRAIL STASH feature
|
||||
// 5.2.8 - Bugfix: Do not turn off all tracks on change
|
||||
// give better power messages
|
||||
// 5.2.7 - Bugfix: EXRAIL ling segment
|
||||
// - Bugfix: Back out wrongly added const
|
||||
// - Bugfix ESP32: Do not inverse DCX direction signal twice
|
||||
// 5.2.6 - Trackmanager broadcast power state on track mode change
|
||||
// 5.2.5 - Trackmanager: Do not treat TRACK_MODE_ALL as TRACK_MODE_DC
|
||||
// 5.2.4 - LCD macro will not do diag if that duplicates @ to same target.
|
||||
// - Added ROUTE_DISABLED macro in EXRAIL
|
||||
// 5.2.3 - Bugfix: Catch stange input to parser
|
||||
// 5.2.2 - Added option to allow MAX_CHARACTER_ROWS to be defined in config.h
|
||||
// 5.2.1 - Trackmanager rework for simpler structure
|
||||
// 5.2.0 - ESP32: Autoreverse and booster mode support
|
||||
// 5.1.21 - EXRAIL invoke multiple ON handlers for same event
|
||||
// 5.1.20 - EXRAIL Tidy and ROUTE_STATE, ROUTE_CAPTION
|
||||
// 5.1.19 - Only flag 2.2.0.0-dev as broken, not 2.2.0.0
|
||||
// 5.1.18 - TURNOUTL bugfix
|
||||
#define VERSION "5.1.17gw"
|
||||
// 5.1.17gw - Giga support by Travis Farmer, with WifiNINA integrated to see if it works
|
||||
// 5.1.17 - Divide out C for config and D for diag commands
|
||||
// 5.1.16 - Remove I2C address from EXTT_TURNTABLE macro to work with MUX, requires separate HAL macro to create
|
||||
// 5.1.15 - LCC/Adapter support and Exrail feature-compile-out.
|
||||
|
Reference in New Issue
Block a user