mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2025-07-29 18:33:44 +02:00
Compare commits
78 Commits
v4.1.1-Pro
...
TrackManag
Author | SHA1 | Date | |
---|---|---|---|
|
4e177cf744 | ||
|
ff96ed1157 | ||
|
a363f37dc7 | ||
|
11d959c8d0 | ||
|
6ff5e7c7a8 | ||
|
79d6ab1c87 | ||
|
2d52d88688 | ||
|
016bc37b53 | ||
|
06e7ad5c53 | ||
|
1c78792dda | ||
|
cfcd61174d | ||
|
55561188e1 | ||
|
55196c2e7d | ||
|
c7b38170c1 | ||
|
d3b72dc4fc | ||
|
17fb921678 | ||
|
867e3b3930 | ||
|
79ef114c0d | ||
|
6d2a9e3b36 | ||
|
f0e8419fea | ||
|
8f9da49cc8 | ||
|
d7a17b10b4 | ||
|
0268304d41 | ||
|
f66f5785f5 | ||
|
6d382fa0f4 | ||
|
af0d381e45 | ||
|
4a56998553 | ||
|
ac32cd5528 | ||
|
e721457844 | ||
|
3e8649f9a1 | ||
|
6994139e57 | ||
|
7a2fd90bfc | ||
|
43bac3f78e | ||
|
cd0b8790b6 | ||
|
228553013b | ||
|
acd6e7560f | ||
|
5bdbe3895d | ||
|
bcd1335b08 | ||
|
724dea22d5 | ||
|
21d1f482cf | ||
|
9273265036 | ||
|
8522e05b13 | ||
|
1b0d700009 | ||
|
aaa3e7a83c | ||
|
ece342f037 | ||
|
f9e36e6693 | ||
|
7dd680ccd5 | ||
|
ef50665c16 | ||
|
3f283620d3 | ||
|
5adabcd1af | ||
|
2727332be3 | ||
|
49e0a0e5f5 | ||
|
78810d0e36 | ||
|
a10fca2b12 | ||
|
99c7ff6c3f | ||
|
2a87a6e997 | ||
|
dea55fec79 | ||
|
865c8dd3bd | ||
|
be186b967b | ||
|
4f2dc0934f | ||
|
75b16c9047 | ||
|
cd952c6ede | ||
|
7e3dcb8e8c | ||
|
4437f870b6 | ||
|
f7e2c0ca99 | ||
|
2890a7928b | ||
|
03372f21e2 | ||
|
678cccbd95 | ||
|
524afc6caf | ||
|
6fc223d80b | ||
|
dd9152864b | ||
|
4f781074eb | ||
|
b29b8c999e | ||
|
99e636974a | ||
|
74bbe595fc | ||
|
1afb4753ec | ||
|
a7740d652d | ||
|
8db937e985 |
1
.gitignore
vendored
1
.gitignore
vendored
@@ -16,3 +16,4 @@ myFilter.cpp
|
|||||||
myAutomation.h
|
myAutomation.h
|
||||||
myFilter.cpp
|
myFilter.cpp
|
||||||
myLayout.h
|
myLayout.h
|
||||||
|
.vscode/extensions.json
|
||||||
|
@@ -28,6 +28,7 @@
|
|||||||
#include "defines.h"
|
#include "defines.h"
|
||||||
#include "DCCWaveform.h"
|
#include "DCCWaveform.h"
|
||||||
#include "DCC.h"
|
#include "DCC.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
|
|
||||||
#if defined(BIG_MEMORY) | defined(WIFI_ON) | defined(ETHERNET_ON)
|
#if defined(BIG_MEMORY) | defined(WIFI_ON) | defined(ETHERNET_ON)
|
||||||
// This section of CommandDistributor is simply not relevant on a uno or similar
|
// This section of CommandDistributor is simply not relevant on a uno or similar
|
||||||
@@ -119,9 +120,9 @@ void CommandDistributor::broadcastLoco(byte slot) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void CommandDistributor::broadcastPower() {
|
void CommandDistributor::broadcastPower() {
|
||||||
bool main=DCCWaveform::mainTrack.getPowerMode()==POWERMODE::ON;
|
bool main=TrackManager::getMainPower()==POWERMODE::ON;
|
||||||
bool prog=DCCWaveform::progTrack.getPowerMode()==POWERMODE::ON;
|
bool prog=TrackManager::getProgPower()==POWERMODE::ON;
|
||||||
bool join=DCCWaveform::progTrackSyncMain;
|
bool join=TrackManager::isJoined();
|
||||||
const FSH * reason=F("");
|
const FSH * reason=F("");
|
||||||
char state='1';
|
char state='1';
|
||||||
if (main && prog && join) reason=F(" JOIN");
|
if (main && prog && join) reason=F(" JOIN");
|
||||||
|
@@ -84,12 +84,15 @@ void setup()
|
|||||||
EthernetInterface::setup();
|
EthernetInterface::setup();
|
||||||
#endif // ETHERNET_ON
|
#endif // ETHERNET_ON
|
||||||
|
|
||||||
|
// Initialise HAL layer before reading EEprom or setting up MotorDrivers
|
||||||
|
IODevice::begin();
|
||||||
|
|
||||||
// Responsibility 3: Start the DCC engine.
|
// Responsibility 3: Start the DCC engine.
|
||||||
// Note: this provides DCC with two motor drivers, main and prog, which handle the motor shield(s)
|
// Note: this provides DCC with two motor drivers, main and prog, which handle the motor shield(s)
|
||||||
// Standard supported devices have pre-configured macros but custome hardware installations require
|
// Standard supported devices have pre-configured macros but custome hardware installations require
|
||||||
// detailed pin mappings and may also require modified subclasses of the MotorDriver to implement specialist logic.
|
// detailed pin mappings and may also require modified subclasses of the MotorDriver to implement specialist logic.
|
||||||
// STANDARD_MOTOR_SHIELD, POLOLU_MOTOR_SHIELD, FIREBOX_MK1, FIREBOX_MK1S are pre defined in MotorShields.h
|
// STANDARD_MOTOR_SHIELD, POLOLU_MOTOR_SHIELD, FIREBOX_MK1, FIREBOX_MK1S are pre defined in MotorShields.h
|
||||||
DCC::begin(MOTOR_SHIELD_TYPE);
|
TrackManager::Setup(MOTOR_SHIELD_TYPE);
|
||||||
|
|
||||||
// Start RMFT aka EX-RAIL (ignored if no automnation)
|
// Start RMFT aka EX-RAIL (ignored if no automnation)
|
||||||
RMFT::begin();
|
RMFT::begin();
|
||||||
@@ -147,7 +150,7 @@ void loop()
|
|||||||
// Report any decrease in memory (will automatically trigger on first call)
|
// Report any decrease in memory (will automatically trigger on first call)
|
||||||
static int ramLowWatermark = __INT_MAX__; // replaced on first loop
|
static int ramLowWatermark = __INT_MAX__; // replaced on first loop
|
||||||
|
|
||||||
int freeNow = minimumFreeMemory();
|
int freeNow = DCCTimer::getMinimumFreeMemory();
|
||||||
if (freeNow < ramLowWatermark) {
|
if (freeNow < ramLowWatermark) {
|
||||||
ramLowWatermark = freeNow;
|
ramLowWatermark = freeNow;
|
||||||
LCD(3,F("Free RAM=%5db"), ramLowWatermark);
|
LCD(3,F("Free RAM=%5db"), ramLowWatermark);
|
||||||
|
430
DCC.cpp
430
DCC.cpp
@@ -8,7 +8,7 @@
|
|||||||
* © 2020-2021 Chris Harlow
|
* © 2020-2021 Chris Harlow
|
||||||
* All rights reserved.
|
* All rights reserved.
|
||||||
*
|
*
|
||||||
* This file is part of Asbelos DCC API
|
* This file is part of DCC-EX
|
||||||
*
|
*
|
||||||
* This is free software: you can redistribute it and/or modify
|
* This is free software: you can redistribute it and/or modify
|
||||||
* it under the terms of the GNU General Public License as published by
|
* it under the terms of the GNU General Public License as published by
|
||||||
@@ -35,6 +35,8 @@
|
|||||||
#include "IODevice.h"
|
#include "IODevice.h"
|
||||||
#include "EXRAIL2.h"
|
#include "EXRAIL2.h"
|
||||||
#include "CommandDistributor.h"
|
#include "CommandDistributor.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
|
||||||
// This module is responsible for converting API calls into
|
// This module is responsible for converting API calls into
|
||||||
// messages to be sent to the waveform generator.
|
// messages to be sent to the waveform generator.
|
||||||
@@ -56,36 +58,26 @@ const byte FN_GROUP_4=0x08;
|
|||||||
const byte FN_GROUP_5=0x10;
|
const byte FN_GROUP_5=0x10;
|
||||||
|
|
||||||
FSH* DCC::shieldName=NULL;
|
FSH* DCC::shieldName=NULL;
|
||||||
byte DCC::joinRelay=UNUSED_PIN;
|
|
||||||
byte DCC::globalSpeedsteps=128;
|
byte DCC::globalSpeedsteps=128;
|
||||||
|
|
||||||
void DCC::begin(const FSH * motorShieldName, MotorDriver * mainDriver, MotorDriver* progDriver) {
|
void DCC::begin(const FSH * motorShieldName) {
|
||||||
shieldName=(FSH *)motorShieldName;
|
shieldName=(FSH *)motorShieldName;
|
||||||
StringFormatter::send(Serial,F("<iDCC-EX V-%S / %S / %S G-%S>\n"), F(VERSION), F(ARDUINO_TYPE), shieldName, F(GITHUB_SHA));
|
StringFormatter::send(Serial,F("<iDCC-EX V-%S / %S / %S G-%S>\n"), F(VERSION), F(ARDUINO_TYPE), shieldName, F(GITHUB_SHA));
|
||||||
|
|
||||||
// Initialise HAL layer before reading EEprom.
|
|
||||||
IODevice::begin();
|
|
||||||
|
|
||||||
#ifndef DISABLE_EEPROM
|
#ifndef DISABLE_EEPROM
|
||||||
// Load stuff from EEprom
|
// Load stuff from EEprom
|
||||||
(void)EEPROM; // tell compiler not to warn this is unused
|
(void)EEPROM; // tell compiler not to warn this is unused
|
||||||
EEStore::init();
|
EEStore::init();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
DCCWaveform::begin(mainDriver,progDriver);
|
DCCWaveform::begin();
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::setJoinRelayPin(byte joinRelayPin) {
|
|
||||||
joinRelay=joinRelayPin;
|
|
||||||
if (joinRelay!=UNUSED_PIN) {
|
|
||||||
pinMode(joinRelay,OUTPUT);
|
|
||||||
digitalWrite(joinRelay,LOW); // LOW is relay disengaged
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCC::setThrottle( uint16_t cab, uint8_t tSpeed, bool tDirection) {
|
void DCC::setThrottle( uint16_t cab, uint8_t tSpeed, bool tDirection) {
|
||||||
byte speedCode = (tSpeed & 0x7F) + tDirection * 128;
|
byte speedCode = (tSpeed & 0x7F) + tDirection * 128;
|
||||||
setThrottle2(cab, speedCode);
|
setThrottle2(cab, speedCode);
|
||||||
|
TrackManager::setDCSignal(cab,speedCode); // in case this is a dcc track on this addr
|
||||||
// retain speed for loco reminders
|
// retain speed for loco reminders
|
||||||
updateLocoReminder(cab, speedCode );
|
updateLocoReminder(cab, speedCode );
|
||||||
}
|
}
|
||||||
@@ -145,12 +137,25 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2) {
|
|||||||
DCCWaveform::mainTrack.schedulePacket(b, nB, 0);
|
DCCWaveform::mainTrack.schedulePacket(b, nB, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t DCC::getThrottleSpeed(int cab) {
|
// returns speed steps 0 to 127 (1 == emergency stop)
|
||||||
|
// or -1 on "loco not found"
|
||||||
|
int8_t DCC::getThrottleSpeed(int cab) {
|
||||||
int reg=lookupSpeedTable(cab);
|
int reg=lookupSpeedTable(cab);
|
||||||
if (reg<0) return -1;
|
if (reg<0) return -1;
|
||||||
return speedTable[reg].speedCode & 0x7F;
|
return speedTable[reg].speedCode & 0x7F;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// returns speed code byte
|
||||||
|
// or 128 (speed 0, dir forward) on "loco not found".
|
||||||
|
uint8_t DCC::getThrottleSpeedByte(int cab) {
|
||||||
|
int reg=lookupSpeedTable(cab);
|
||||||
|
if (reg<0)
|
||||||
|
return 128;
|
||||||
|
return speedTable[reg].speedCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
// returns direction on loco
|
||||||
|
// or true/forward on "loco not found"
|
||||||
bool DCC::getThrottleDirection(int cab) {
|
bool DCC::getThrottleDirection(int cab) {
|
||||||
int reg=lookupSpeedTable(cab);
|
int reg=lookupSpeedTable(cab);
|
||||||
if (reg<0) return true;
|
if (reg<0) return true;
|
||||||
@@ -237,24 +242,39 @@ uint32_t DCC::getFunctionMap(int cab) {
|
|||||||
return (reg<0)?0:speedTable[reg].functions;
|
return (reg<0)?0:speedTable[reg].functions;
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::setAccessory(int address, byte number, bool activate) {
|
void DCC::setAccessory(int address, byte port, bool gate, byte onoff /*= 2*/) {
|
||||||
|
// onoff is tristate:
|
||||||
|
// 0 => send off packet
|
||||||
|
// 1 => send on packet
|
||||||
|
// >1 => send both on and off packets.
|
||||||
|
|
||||||
|
// An accessory has an address, 4 ports and 2 gates (coils) each. That's how
|
||||||
|
// the initial decoders were orgnized and that influenced how the DCC
|
||||||
|
// standard was made.
|
||||||
#ifdef DIAG_IO
|
#ifdef DIAG_IO
|
||||||
DIAG(F("DCC::setAccessory(%d,%d,%d)"), address, number, activate);
|
DIAG(F("DCC::setAccessory(%d,%d,%d)"), address, port, gate);
|
||||||
#endif
|
#endif
|
||||||
// use masks to detect wrong values and do nothing
|
// use masks to detect wrong values and do nothing
|
||||||
if(address != (address & 511))
|
if(address != (address & 511))
|
||||||
return;
|
return;
|
||||||
if(number != (number & 3))
|
if(port != (port & 3))
|
||||||
return;
|
return;
|
||||||
byte b[2];
|
byte b[2];
|
||||||
|
|
||||||
b[0] = address % 64 + 128; // first byte is of the form 10AAAAAA, where AAAAAA represent 6 least signifcant bits of accessory address
|
// first byte is of the form 10AAAAAA, where AAAAAA represent 6 least signifcant bits of accessory address
|
||||||
b[1] = ((((address / 64) % 8) << 4) + (number % 4 << 1) + activate % 2) ^ 0xF8; // second byte is of the form 1AAACDDD, where C should be 1, and the least significant D represent activate/deactivate
|
// second byte is of the form 1AAACPPG, where C is 1 for on, PP the ports 0 to 3 and G the gate (coil).
|
||||||
|
b[0] = address % 64 + 128;
|
||||||
DCCWaveform::mainTrack.schedulePacket(b, 2, 4); // Repeat the packet four times
|
b[1] = ((((address / 64) % 8) << 4) + (port % 4 << 1) + gate % 2) ^ 0xF8;
|
||||||
|
if (onoff != 0) {
|
||||||
|
DCCWaveform::mainTrack.schedulePacket(b, 2, 3); // Repeat on packet three times
|
||||||
#if defined(EXRAIL_ACTIVE)
|
#if defined(EXRAIL_ACTIVE)
|
||||||
RMFT2::activateEvent(address<<2|number,activate);
|
RMFT2::activateEvent(address<<2|port,gate);
|
||||||
#endif
|
#endif
|
||||||
|
}
|
||||||
|
if (onoff != 1) {
|
||||||
|
b[1] &= ~0x08; // set C to 0
|
||||||
|
DCCWaveform::mainTrack.schedulePacket(b, 2, 3); // Repeat off packet three times
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
@@ -296,14 +316,6 @@ void DCC::writeCVBitMain(int cab, int cv, byte bNum, bool bValue) {
|
|||||||
DCCWaveform::mainTrack.schedulePacket(b, nB, 4);
|
DCCWaveform::mainTrack.schedulePacket(b, nB, 4);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::setProgTrackSyncMain(bool on) {
|
|
||||||
if (joinRelay!=UNUSED_PIN) digitalWrite(joinRelay,on?HIGH:LOW);
|
|
||||||
DCCWaveform::progTrackSyncMain=on;
|
|
||||||
}
|
|
||||||
void DCC::setProgTrackBoost(bool on) {
|
|
||||||
DCCWaveform::progTrackBoosted=on;
|
|
||||||
}
|
|
||||||
|
|
||||||
FSH* DCC::getMotorShieldName() {
|
FSH* DCC::getMotorShieldName() {
|
||||||
return shieldName;
|
return shieldName;
|
||||||
}
|
}
|
||||||
@@ -313,14 +325,14 @@ const ackOp FLASH WRITE_BIT0_PROG[] = {
|
|||||||
W0,WACK,
|
W0,WACK,
|
||||||
V0, WACK, // validate bit is 0
|
V0, WACK, // validate bit is 0
|
||||||
ITC1, // if acked, callback(1)
|
ITC1, // if acked, callback(1)
|
||||||
FAIL // callback (-1)
|
CALLFAIL // callback (-1)
|
||||||
};
|
};
|
||||||
const ackOp FLASH WRITE_BIT1_PROG[] = {
|
const ackOp FLASH WRITE_BIT1_PROG[] = {
|
||||||
BASELINE,
|
BASELINE,
|
||||||
W1,WACK,
|
W1,WACK,
|
||||||
V1, WACK, // validate bit is 1
|
V1, WACK, // validate bit is 1
|
||||||
ITC1, // if acked, callback(1)
|
ITC1, // if acked, callback(1)
|
||||||
FAIL // callback (-1)
|
CALLFAIL // callback (-1)
|
||||||
};
|
};
|
||||||
|
|
||||||
const ackOp FLASH VERIFY_BIT0_PROG[] = {
|
const ackOp FLASH VERIFY_BIT0_PROG[] = {
|
||||||
@@ -329,7 +341,7 @@ const ackOp FLASH VERIFY_BIT0_PROG[] = {
|
|||||||
ITC0, // if acked, callback(0)
|
ITC0, // if acked, callback(0)
|
||||||
V1, WACK, // validate bit is 1
|
V1, WACK, // validate bit is 1
|
||||||
ITC1,
|
ITC1,
|
||||||
FAIL // callback (-1)
|
CALLFAIL // callback (-1)
|
||||||
};
|
};
|
||||||
const ackOp FLASH VERIFY_BIT1_PROG[] = {
|
const ackOp FLASH VERIFY_BIT1_PROG[] = {
|
||||||
BASELINE,
|
BASELINE,
|
||||||
@@ -337,7 +349,7 @@ const ackOp FLASH VERIFY_BIT1_PROG[] = {
|
|||||||
ITC1, // if acked, callback(1)
|
ITC1, // if acked, callback(1)
|
||||||
V0, WACK,
|
V0, WACK,
|
||||||
ITC0,
|
ITC0,
|
||||||
FAIL // callback (-1)
|
CALLFAIL // callback (-1)
|
||||||
};
|
};
|
||||||
|
|
||||||
const ackOp FLASH READ_BIT_PROG[] = {
|
const ackOp FLASH READ_BIT_PROG[] = {
|
||||||
@@ -346,7 +358,7 @@ const ackOp FLASH READ_BIT_PROG[] = {
|
|||||||
ITC1, // if acked, callback(1)
|
ITC1, // if acked, callback(1)
|
||||||
V0, WACK, // validate bit is zero
|
V0, WACK, // validate bit is zero
|
||||||
ITC0, // if acked callback 0
|
ITC0, // if acked callback 0
|
||||||
FAIL // bit not readable
|
CALLFAIL // bit not readable
|
||||||
};
|
};
|
||||||
|
|
||||||
const ackOp FLASH WRITE_BYTE_PROG[] = {
|
const ackOp FLASH WRITE_BYTE_PROG[] = {
|
||||||
@@ -354,7 +366,7 @@ const ackOp FLASH WRITE_BYTE_PROG[] = {
|
|||||||
WB,WACK,ITC1, // Write and callback(1) if ACK
|
WB,WACK,ITC1, // Write and callback(1) if ACK
|
||||||
// handle decoders that dont ack a write
|
// handle decoders that dont ack a write
|
||||||
VB,WACK,ITC1, // validate byte and callback(1) if correct
|
VB,WACK,ITC1, // validate byte and callback(1) if correct
|
||||||
FAIL // callback (-1)
|
CALLFAIL // callback (-1)
|
||||||
};
|
};
|
||||||
|
|
||||||
const ackOp FLASH VERIFY_BYTE_PROG[] = {
|
const ackOp FLASH VERIFY_BYTE_PROG[] = {
|
||||||
@@ -380,7 +392,7 @@ const ackOp FLASH VERIFY_BYTE_PROG[] = {
|
|||||||
V0, WACK, MERGE,
|
V0, WACK, MERGE,
|
||||||
V0, WACK, MERGE,
|
V0, WACK, MERGE,
|
||||||
VB, WACK, ITCBV, // verify merged byte and return it if acked ok - with retry report
|
VB, WACK, ITCBV, // verify merged byte and return it if acked ok - with retry report
|
||||||
FAIL };
|
CALLFAIL };
|
||||||
|
|
||||||
|
|
||||||
const ackOp FLASH READ_CV_PROG[] = {
|
const ackOp FLASH READ_CV_PROG[] = {
|
||||||
@@ -403,7 +415,7 @@ const ackOp FLASH READ_CV_PROG[] = {
|
|||||||
V0, WACK, MERGE,
|
V0, WACK, MERGE,
|
||||||
V0, WACK, MERGE,
|
V0, WACK, MERGE,
|
||||||
VB, WACK, ITCB, // verify merged byte and return it if acked ok
|
VB, WACK, ITCB, // verify merged byte and return it if acked ok
|
||||||
FAIL }; // verification failed
|
CALLFAIL }; // verification failed
|
||||||
|
|
||||||
|
|
||||||
const ackOp FLASH LOCO_ID_PROG[] = {
|
const ackOp FLASH LOCO_ID_PROG[] = {
|
||||||
@@ -469,7 +481,7 @@ const ackOp FLASH LOCO_ID_PROG[] = {
|
|||||||
V0, WACK, MERGE,
|
V0, WACK, MERGE,
|
||||||
V0, WACK, MERGE,
|
V0, WACK, MERGE,
|
||||||
VB, WACK, ITCB, // verify merged byte and callback
|
VB, WACK, ITCB, // verify merged byte and callback
|
||||||
FAIL
|
CALLFAIL
|
||||||
};
|
};
|
||||||
|
|
||||||
const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
|
const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
|
||||||
@@ -486,7 +498,7 @@ const ackOp FLASH SHORT_LOCO_ID_PROG[] = {
|
|||||||
SETBYTEL, // low byte of word
|
SETBYTEL, // low byte of word
|
||||||
WB,WACK, // some decoders don't ACK writes
|
WB,WACK, // some decoders don't ACK writes
|
||||||
VB,WACK,ITCB,
|
VB,WACK,ITCB,
|
||||||
FAIL
|
CALLFAIL
|
||||||
};
|
};
|
||||||
|
|
||||||
const ackOp FLASH LONG_LOCO_ID_PROG[] = {
|
const ackOp FLASH LONG_LOCO_ID_PROG[] = {
|
||||||
@@ -510,39 +522,39 @@ const ackOp FLASH LONG_LOCO_ID_PROG[] = {
|
|||||||
SETBYTEL, // low byte of word
|
SETBYTEL, // low byte of word
|
||||||
WB,WACK,
|
WB,WACK,
|
||||||
VB,WACK,ITC1, // callback(1) means Ok
|
VB,WACK,ITC1, // callback(1) means Ok
|
||||||
FAIL
|
CALLFAIL
|
||||||
};
|
};
|
||||||
|
|
||||||
void DCC::writeCVByte(int16_t cv, byte byteValue, ACK_CALLBACK callback) {
|
void DCC::writeCVByte(int16_t cv, byte byteValue, ACK_CALLBACK callback) {
|
||||||
ackManagerSetup(cv, byteValue, WRITE_BYTE_PROG, callback);
|
DCCACK::Setup(cv, byteValue, WRITE_BYTE_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::writeCVBit(int16_t cv, byte bitNum, bool bitValue, ACK_CALLBACK callback) {
|
void DCC::writeCVBit(int16_t cv, byte bitNum, bool bitValue, ACK_CALLBACK callback) {
|
||||||
if (bitNum >= 8) callback(-1);
|
if (bitNum >= 8) callback(-1);
|
||||||
else ackManagerSetup(cv, bitNum, bitValue?WRITE_BIT1_PROG:WRITE_BIT0_PROG, callback);
|
else DCCACK::Setup(cv, bitNum, bitValue?WRITE_BIT1_PROG:WRITE_BIT0_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::verifyCVByte(int16_t cv, byte byteValue, ACK_CALLBACK callback) {
|
void DCC::verifyCVByte(int16_t cv, byte byteValue, ACK_CALLBACK callback) {
|
||||||
ackManagerSetup(cv, byteValue, VERIFY_BYTE_PROG, callback);
|
DCCACK::Setup(cv, byteValue, VERIFY_BYTE_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::verifyCVBit(int16_t cv, byte bitNum, bool bitValue, ACK_CALLBACK callback) {
|
void DCC::verifyCVBit(int16_t cv, byte bitNum, bool bitValue, ACK_CALLBACK callback) {
|
||||||
if (bitNum >= 8) callback(-1);
|
if (bitNum >= 8) callback(-1);
|
||||||
else ackManagerSetup(cv, bitNum, bitValue?VERIFY_BIT1_PROG:VERIFY_BIT0_PROG, callback);
|
else DCCACK::Setup(cv, bitNum, bitValue?VERIFY_BIT1_PROG:VERIFY_BIT0_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void DCC::readCVBit(int16_t cv, byte bitNum, ACK_CALLBACK callback) {
|
void DCC::readCVBit(int16_t cv, byte bitNum, ACK_CALLBACK callback) {
|
||||||
if (bitNum >= 8) callback(-1);
|
if (bitNum >= 8) callback(-1);
|
||||||
else ackManagerSetup(cv, bitNum,READ_BIT_PROG, callback);
|
else DCCACK::Setup(cv, bitNum,READ_BIT_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::readCV(int16_t cv, ACK_CALLBACK callback) {
|
void DCC::readCV(int16_t cv, ACK_CALLBACK callback) {
|
||||||
ackManagerSetup(cv, 0,READ_CV_PROG, callback);
|
DCCACK::Setup(cv, 0,READ_CV_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::getLocoId(ACK_CALLBACK callback) {
|
void DCC::getLocoId(ACK_CALLBACK callback) {
|
||||||
ackManagerSetup(0,0, LOCO_ID_PROG, callback);
|
DCCACK::Setup(0,0, LOCO_ID_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::setLocoId(int id,ACK_CALLBACK callback) {
|
void DCC::setLocoId(int id,ACK_CALLBACK callback) {
|
||||||
@@ -551,9 +563,9 @@ void DCC::setLocoId(int id,ACK_CALLBACK callback) {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (id<=HIGHEST_SHORT_ADDR)
|
if (id<=HIGHEST_SHORT_ADDR)
|
||||||
ackManagerSetup(id, SHORT_LOCO_ID_PROG, callback);
|
DCCACK::Setup(id, SHORT_LOCO_ID_PROG, callback);
|
||||||
else
|
else
|
||||||
ackManagerSetup(id | 0xc000,LONG_LOCO_ID_PROG, callback);
|
DCCACK::Setup(id | 0xc000,LONG_LOCO_ID_PROG, callback);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCC::forgetLoco(int cab) { // removes any speed reminders for this loco
|
void DCC::forgetLoco(int cab) { // removes any speed reminders for this loco
|
||||||
@@ -570,8 +582,7 @@ void DCC::forgetAllLocos() { // removes all speed reminders
|
|||||||
byte DCC::loopStatus=0;
|
byte DCC::loopStatus=0;
|
||||||
|
|
||||||
void DCC::loop() {
|
void DCC::loop() {
|
||||||
DCCWaveform::loop(ackManagerProg!=NULL); // power overload checks
|
TrackManager::loop(); // power overload checks
|
||||||
ackManagerLoop(); // maintain prog track ack manager
|
|
||||||
issueReminders();
|
issueReminders();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -698,319 +709,6 @@ void DCC::updateLocoReminder(int loco, byte speedCode) {
|
|||||||
DCC::LOCO DCC::speedTable[MAX_LOCOS];
|
DCC::LOCO DCC::speedTable[MAX_LOCOS];
|
||||||
int DCC::nextLoco = 0;
|
int DCC::nextLoco = 0;
|
||||||
|
|
||||||
//ACK MANAGER
|
|
||||||
ackOp const * DCC::ackManagerProg;
|
|
||||||
ackOp const * DCC::ackManagerProgStart;
|
|
||||||
byte DCC::ackManagerByte;
|
|
||||||
byte DCC::ackManagerByteVerify;
|
|
||||||
byte DCC::ackManagerStash;
|
|
||||||
int DCC::ackManagerWord;
|
|
||||||
byte DCC::ackManagerRetry;
|
|
||||||
byte DCC::ackRetry = 2;
|
|
||||||
int16_t DCC::ackRetrySum;
|
|
||||||
int16_t DCC::ackRetryPSum;
|
|
||||||
int DCC::ackManagerCv;
|
|
||||||
byte DCC::ackManagerBitNum;
|
|
||||||
bool DCC::ackReceived;
|
|
||||||
bool DCC::ackManagerRejoin;
|
|
||||||
|
|
||||||
CALLBACK_STATE DCC::callbackState=READY;
|
|
||||||
|
|
||||||
ACK_CALLBACK DCC::ackManagerCallback;
|
|
||||||
|
|
||||||
void DCC::ackManagerSetup(int cv, byte byteValueOrBitnum, ackOp const program[], ACK_CALLBACK callback) {
|
|
||||||
if (!DCCWaveform::progTrack.canMeasureCurrent()) {
|
|
||||||
callback(-2);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
ackManagerRejoin=DCCWaveform::progTrackSyncMain;
|
|
||||||
if (ackManagerRejoin ) {
|
|
||||||
// Change from JOIN must zero resets packet.
|
|
||||||
setProgTrackSyncMain(false);
|
|
||||||
DCCWaveform::progTrack.sentResetsSincePacket = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
DCCWaveform::progTrack.autoPowerOff=false;
|
|
||||||
if (DCCWaveform::progTrack.getPowerMode() == POWERMODE::OFF) {
|
|
||||||
DCCWaveform::progTrack.autoPowerOff=true; // power off afterwards
|
|
||||||
if (Diag::ACK) DIAG(F("Auto Prog power on"));
|
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::ON);
|
|
||||||
if (MotorDriver::commonFaultPin)
|
|
||||||
DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON);
|
|
||||||
DCCWaveform::progTrack.sentResetsSincePacket = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
ackManagerCv = cv;
|
|
||||||
ackManagerProg = program;
|
|
||||||
ackManagerProgStart = program;
|
|
||||||
ackManagerRetry = ackRetry;
|
|
||||||
ackManagerByte = byteValueOrBitnum;
|
|
||||||
ackManagerByteVerify = byteValueOrBitnum;
|
|
||||||
ackManagerBitNum=byteValueOrBitnum;
|
|
||||||
ackManagerCallback = callback;
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCC::ackManagerSetup(int wordval, ackOp const program[], ACK_CALLBACK callback) {
|
|
||||||
ackManagerWord=wordval;
|
|
||||||
ackManagerSetup(0, 0, program, callback);
|
|
||||||
}
|
|
||||||
|
|
||||||
const byte RESET_MIN=8; // tuning of reset counter before sending message
|
|
||||||
|
|
||||||
// checkRessets return true if the caller should yield back to loop and try later.
|
|
||||||
bool DCC::checkResets(uint8_t numResets) {
|
|
||||||
return DCCWaveform::progTrack.sentResetsSincePacket < numResets;
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCC::ackManagerLoop() {
|
|
||||||
while (ackManagerProg) {
|
|
||||||
byte opcode=GETFLASH(ackManagerProg);
|
|
||||||
|
|
||||||
// breaks from this switch will step to next prog entry
|
|
||||||
// returns from this switch will stay on same entry
|
|
||||||
// (typically waiting for a reset counter or ACK waiting, or when all finished.)
|
|
||||||
switch (opcode) {
|
|
||||||
case BASELINE:
|
|
||||||
if (DCCWaveform::progTrack.getPowerMode()==POWERMODE::OVERLOAD) return;
|
|
||||||
if (checkResets(DCCWaveform::progTrack.autoPowerOff || ackManagerRejoin ? 20 : 3)) return;
|
|
||||||
DCCWaveform::progTrack.setAckBaseline();
|
|
||||||
callbackState=READY;
|
|
||||||
break;
|
|
||||||
case W0: // write 0 bit
|
|
||||||
case W1: // write 1 bit
|
|
||||||
{
|
|
||||||
if (checkResets(RESET_MIN)) return;
|
|
||||||
if (Diag::ACK) DIAG(F("W%d cv=%d bit=%d"),opcode==W1, ackManagerCv,ackManagerBitNum);
|
|
||||||
byte instruction = WRITE_BIT | (opcode==W1 ? BIT_ON : BIT_OFF) | ackManagerBitNum;
|
|
||||||
byte message[] = {cv1(BIT_MANIPULATE, ackManagerCv), cv2(ackManagerCv), instruction };
|
|
||||||
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
|
||||||
DCCWaveform::progTrack.setAckPending();
|
|
||||||
callbackState=AFTER_WRITE;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case WB: // write byte
|
|
||||||
{
|
|
||||||
if (checkResets( RESET_MIN)) return;
|
|
||||||
if (Diag::ACK) DIAG(F("WB cv=%d value=%d"),ackManagerCv,ackManagerByte);
|
|
||||||
byte message[] = {cv1(WRITE_BYTE, ackManagerCv), cv2(ackManagerCv), ackManagerByte};
|
|
||||||
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
|
||||||
DCCWaveform::progTrack.setAckPending();
|
|
||||||
callbackState=AFTER_WRITE;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case VB: // Issue validate Byte packet
|
|
||||||
{
|
|
||||||
if (checkResets( RESET_MIN)) return;
|
|
||||||
if (Diag::ACK) DIAG(F("VB cv=%d value=%d"),ackManagerCv,ackManagerByte);
|
|
||||||
byte message[] = { cv1(VERIFY_BYTE, ackManagerCv), cv2(ackManagerCv), ackManagerByte};
|
|
||||||
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
|
||||||
DCCWaveform::progTrack.setAckPending();
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case V0:
|
|
||||||
case V1: // Issue validate bit=0 or bit=1 packet
|
|
||||||
{
|
|
||||||
if (checkResets(RESET_MIN)) return;
|
|
||||||
if (Diag::ACK) DIAG(F("V%d cv=%d bit=%d"),opcode==V1, ackManagerCv,ackManagerBitNum);
|
|
||||||
byte instruction = VERIFY_BIT | (opcode==V0?BIT_OFF:BIT_ON) | ackManagerBitNum;
|
|
||||||
byte message[] = {cv1(BIT_MANIPULATE, ackManagerCv), cv2(ackManagerCv), instruction };
|
|
||||||
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
|
||||||
DCCWaveform::progTrack.setAckPending();
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case WACK: // wait for ack (or absence of ack)
|
|
||||||
{
|
|
||||||
byte ackState=2; // keep polling
|
|
||||||
|
|
||||||
ackState=DCCWaveform::progTrack.getAck();
|
|
||||||
if (ackState==2) return; // keep polling
|
|
||||||
ackReceived=ackState==1;
|
|
||||||
break; // we have a genuine ACK result
|
|
||||||
}
|
|
||||||
case ITC0:
|
|
||||||
case ITC1: // If True Callback(0 or 1) (if prevous WACK got an ACK)
|
|
||||||
if (ackReceived) {
|
|
||||||
callback(opcode==ITC0?0:1);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case ITCB: // If True callback(byte)
|
|
||||||
if (ackReceived) {
|
|
||||||
callback(ackManagerByte);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case ITCBV: // If True callback(byte) - Verify
|
|
||||||
if (ackReceived) {
|
|
||||||
if (ackManagerByte == ackManagerByteVerify) {
|
|
||||||
ackRetrySum ++;
|
|
||||||
LCD(1, F("v %d %d Sum=%d"), ackManagerCv, ackManagerByte, ackRetrySum);
|
|
||||||
}
|
|
||||||
callback(ackManagerByte);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case ITCB7: // If True callback(byte & 0x7F)
|
|
||||||
if (ackReceived) {
|
|
||||||
callback(ackManagerByte & 0x7F);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case NAKFAIL: // If nack callback(-1)
|
|
||||||
if (!ackReceived) {
|
|
||||||
callback(-1);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case FAIL: // callback(-1)
|
|
||||||
callback(-1);
|
|
||||||
return;
|
|
||||||
|
|
||||||
case BIV: // ackManagerByte initial value
|
|
||||||
ackManagerByte = ackManagerByteVerify;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case STARTMERGE:
|
|
||||||
ackManagerBitNum=7;
|
|
||||||
ackManagerByte=0;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case MERGE: // Merge previous Validate zero wack response with byte value and update bit number (use for reading CV bytes)
|
|
||||||
ackManagerByte <<= 1;
|
|
||||||
// ackReceived means bit is zero.
|
|
||||||
if (!ackReceived) ackManagerByte |= 1;
|
|
||||||
ackManagerBitNum--;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case SETBIT:
|
|
||||||
ackManagerProg++;
|
|
||||||
ackManagerBitNum=GETFLASH(ackManagerProg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case SETCV:
|
|
||||||
ackManagerProg++;
|
|
||||||
ackManagerCv=GETFLASH(ackManagerProg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case SETBYTE:
|
|
||||||
ackManagerProg++;
|
|
||||||
ackManagerByte=GETFLASH(ackManagerProg);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case SETBYTEH:
|
|
||||||
ackManagerByte=highByte(ackManagerWord);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case SETBYTEL:
|
|
||||||
ackManagerByte=lowByte(ackManagerWord);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case STASHLOCOID:
|
|
||||||
ackManagerStash=ackManagerByte; // stash value from CV17
|
|
||||||
break;
|
|
||||||
|
|
||||||
case COMBINELOCOID:
|
|
||||||
// ackManagerStash is cv17, ackManagerByte is CV 18
|
|
||||||
callback( LONG_ADDR_MARKER | ( ackManagerByte + ((ackManagerStash - 192) << 8)));
|
|
||||||
return;
|
|
||||||
|
|
||||||
case ITSKIP:
|
|
||||||
if (!ackReceived) break;
|
|
||||||
// SKIP opcodes until SKIPTARGET found
|
|
||||||
while (opcode!=SKIPTARGET) {
|
|
||||||
ackManagerProg++;
|
|
||||||
opcode=GETFLASH(ackManagerProg);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case SKIPTARGET:
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
DIAG(F("!! ackOp %d FAULT!!"),opcode);
|
|
||||||
callback( -1);
|
|
||||||
return;
|
|
||||||
|
|
||||||
} // end of switch
|
|
||||||
ackManagerProg++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCC::callback(int value) {
|
|
||||||
// check for automatic retry
|
|
||||||
if (value == -1 && ackManagerRetry > 0) {
|
|
||||||
ackRetrySum ++;
|
|
||||||
LCD(0, F("Retry %d %d Sum=%d"), ackManagerCv, ackManagerRetry, ackRetrySum);
|
|
||||||
ackManagerRetry --;
|
|
||||||
ackManagerProg = ackManagerProgStart;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
static unsigned long callbackStart;
|
|
||||||
// We are about to leave programming mode
|
|
||||||
// Rule 1: If we have written to a decoder we must maintain power for 100mS
|
|
||||||
// Rule 2: If we are re-joining the main track we must power off for 30mS
|
|
||||||
|
|
||||||
switch (callbackState) {
|
|
||||||
case AFTER_WRITE: // first attempt to callback after a write operation
|
|
||||||
if (!ackManagerRejoin && !DCCWaveform::progTrack.autoPowerOff) {
|
|
||||||
callbackState=READY;
|
|
||||||
break;
|
|
||||||
} // lines 906-910 added. avoid wait after write. use 1 PROG
|
|
||||||
callbackStart=millis();
|
|
||||||
callbackState=WAITING_100;
|
|
||||||
if (Diag::ACK) DIAG(F("Stable 100mS"));
|
|
||||||
break;
|
|
||||||
|
|
||||||
case WAITING_100: // waiting for 100mS
|
|
||||||
if (millis()-callbackStart < 100) break;
|
|
||||||
// stable after power maintained for 100mS
|
|
||||||
|
|
||||||
// If we are going to power off anyway, it doesnt matter
|
|
||||||
// but if we will keep the power on, we must off it for 30mS
|
|
||||||
if (DCCWaveform::progTrack.autoPowerOff) callbackState=READY;
|
|
||||||
else { // Need to cycle power off and on
|
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::OFF);
|
|
||||||
callbackStart=millis();
|
|
||||||
callbackState=WAITING_30;
|
|
||||||
if (Diag::ACK) DIAG(F("OFF 30mS"));
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case WAITING_30: // waiting for 30mS with power off
|
|
||||||
if (millis()-callbackStart < 30) break;
|
|
||||||
//power has been off for 30mS
|
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::ON);
|
|
||||||
callbackState=READY;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case READY: // ready after read, or write after power delay and off period.
|
|
||||||
// power off if we powered it on
|
|
||||||
if (DCCWaveform::progTrack.autoPowerOff) {
|
|
||||||
if (Diag::ACK) DIAG(F("Auto Prog power off"));
|
|
||||||
DCCWaveform::progTrack.doAutoPowerOff();
|
|
||||||
if (MotorDriver::commonFaultPin)
|
|
||||||
DCCWaveform::mainTrack.setPowerMode(POWERMODE::OFF);
|
|
||||||
}
|
|
||||||
// Restore <1 JOIN> to state before BASELINE
|
|
||||||
if (ackManagerRejoin) {
|
|
||||||
setProgTrackSyncMain(true);
|
|
||||||
if (Diag::ACK) DIAG(F("Auto JOIN"));
|
|
||||||
}
|
|
||||||
|
|
||||||
ackManagerProg=NULL; // no more steps to execute
|
|
||||||
if (Diag::ACK) DIAG(F("Callback(%d)"),value);
|
|
||||||
(ackManagerCallback)( value);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCC::displayCabList(Print * stream) {
|
void DCC::displayCabList(Print * stream) {
|
||||||
|
|
||||||
|
125
DCC.h
125
DCC.h
@@ -36,69 +36,28 @@
|
|||||||
#error short addr greater than 127 does not make sense
|
#error short addr greater than 127 does not make sense
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
#include "DCCACK.h"
|
||||||
const uint16_t LONG_ADDR_MARKER = 0x4000;
|
const uint16_t LONG_ADDR_MARKER = 0x4000;
|
||||||
|
|
||||||
typedef void (*ACK_CALLBACK)(int16_t result);
|
|
||||||
|
|
||||||
enum ackOp : byte
|
|
||||||
{ // Program opcodes for the ack Manager
|
|
||||||
BASELINE, // ensure enough resets sent before starting and obtain baseline current
|
|
||||||
W0,
|
|
||||||
W1, // issue write bit (0..1) packet
|
|
||||||
WB, // issue write byte packet
|
|
||||||
VB, // Issue validate Byte packet
|
|
||||||
V0, // Issue validate bit=0 packet
|
|
||||||
V1, // issue validate bit=1 packlet
|
|
||||||
WACK, // wait for ack (or absence of ack)
|
|
||||||
ITC1, // If True Callback(1) (if prevous WACK got an ACK)
|
|
||||||
ITC0, // If True callback(0);
|
|
||||||
ITCB, // If True callback(byte)
|
|
||||||
ITCBV, // If True callback(byte) - end of Verify Byte
|
|
||||||
ITCB7, // If True callback(byte &0x7F)
|
|
||||||
NAKFAIL, // if false callback(-1)
|
|
||||||
FAIL, // callback(-1)
|
|
||||||
BIV, // Set ackManagerByte to initial value for Verify retry
|
|
||||||
STARTMERGE, // Clear bit and byte settings ready for merge pass
|
|
||||||
MERGE, // Merge previous wack response with byte value and decrement bit number (use for readimng CV bytes)
|
|
||||||
SETBIT, // sets bit number to next prog byte
|
|
||||||
SETCV, // sets cv number to next prog byte
|
|
||||||
SETBYTE, // sets current byte to next prog byte
|
|
||||||
SETBYTEH, // sets current byte to word high byte
|
|
||||||
SETBYTEL, // sets current byte to word low byte
|
|
||||||
STASHLOCOID, // keeps current byte value for later
|
|
||||||
COMBINELOCOID, // combines current value with stashed value and returns it
|
|
||||||
ITSKIP, // skip to SKIPTARGET if ack true
|
|
||||||
SKIPTARGET = 0xFF // jump to target
|
|
||||||
};
|
|
||||||
|
|
||||||
enum CALLBACK_STATE : byte {
|
|
||||||
AFTER_WRITE, // Start callback sequence after something was written to the decoder
|
|
||||||
WAITING_100, // Waiting for 100mS of stable power
|
|
||||||
WAITING_30, // waiting to 30ms of power off gap.
|
|
||||||
READY, // Ready to complete callback
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
// Allocations with memory implications..!
|
// Allocations with memory implications..!
|
||||||
// Base system takes approx 900 bytes + 8 per loco. Turnouts, Sensors etc are dynamically created
|
// Base system takes approx 900 bytes + 8 per loco. Turnouts, Sensors etc are dynamically created
|
||||||
#if defined(ARDUINO_AVR_UNO)
|
#if defined(HAS_ENOUGH_MEMORY)
|
||||||
const byte MAX_LOCOS = 20;
|
|
||||||
#elif defined(ARDUINO_AVR_NANO)
|
|
||||||
const byte MAX_LOCOS = 30;
|
|
||||||
#else
|
|
||||||
const byte MAX_LOCOS = 50;
|
const byte MAX_LOCOS = 50;
|
||||||
|
#else
|
||||||
|
const byte MAX_LOCOS = 30;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
class DCC
|
class DCC
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
static void begin(const FSH * motorShieldName, MotorDriver *mainDriver, MotorDriver *progDriver);
|
static void begin(const FSH * motorShieldName);
|
||||||
static void setJoinRelayPin(byte joinRelayPin);
|
|
||||||
static void loop();
|
static void loop();
|
||||||
|
|
||||||
// Public DCC API functions
|
// Public DCC API functions
|
||||||
static void setThrottle(uint16_t cab, uint8_t tSpeed, bool tDirection);
|
static void setThrottle(uint16_t cab, uint8_t tSpeed, bool tDirection);
|
||||||
static uint8_t getThrottleSpeed(int cab);
|
static int8_t getThrottleSpeed(int cab);
|
||||||
|
static uint8_t getThrottleSpeedByte(int cab);
|
||||||
static bool getThrottleDirection(int cab);
|
static bool getThrottleDirection(int cab);
|
||||||
static void writeCVByteMain(int cab, int cv, byte bValue);
|
static void writeCVByteMain(int cab, int cv, byte bValue);
|
||||||
static void writeCVBitMain(int cab, int cv, byte bNum, bool bValue);
|
static void writeCVBitMain(int cab, int cv, byte bNum, bool bValue);
|
||||||
@@ -108,11 +67,9 @@ public:
|
|||||||
static int getFn(int cab, int16_t functionNumber);
|
static int getFn(int cab, int16_t functionNumber);
|
||||||
static uint32_t getFunctionMap(int cab);
|
static uint32_t getFunctionMap(int cab);
|
||||||
static void updateGroupflags(byte &flags, int16_t functionNumber);
|
static void updateGroupflags(byte &flags, int16_t functionNumber);
|
||||||
static void setAccessory(int aAdd, byte aNum, bool activate);
|
static void setAccessory(int address, byte port, bool gate, byte onoff = 2);
|
||||||
static bool writeTextPacket(byte *b, int nBytes);
|
static bool writeTextPacket(byte *b, int nBytes);
|
||||||
static void setProgTrackSyncMain(bool on); // when true, prog track becomes driveable
|
|
||||||
static void setProgTrackBoost(bool on); // when true, special prog track current limit does not apply
|
|
||||||
|
|
||||||
// ACKable progtrack calls bitresults callback 0,0 or -1, cv returns value or -1
|
// ACKable progtrack calls bitresults callback 0,0 or -1, cv returns value or -1
|
||||||
static void readCV(int16_t cv, ACK_CALLBACK callback);
|
static void readCV(int16_t cv, ACK_CALLBACK callback);
|
||||||
static void readCVBit(int16_t cv, byte bitNum, ACK_CALLBACK callback); // -1 for error
|
static void readCVBit(int16_t cv, byte bitNum, ACK_CALLBACK callback); // -1 for error
|
||||||
@@ -132,13 +89,7 @@ public:
|
|||||||
static inline void setGlobalSpeedsteps(byte s) {
|
static inline void setGlobalSpeedsteps(byte s) {
|
||||||
globalSpeedsteps = s;
|
globalSpeedsteps = s;
|
||||||
};
|
};
|
||||||
static inline int16_t setAckRetry(byte retry) {
|
|
||||||
ackRetry = retry;
|
|
||||||
ackRetryPSum = ackRetrySum;
|
|
||||||
ackRetrySum = 0; // reset running total
|
|
||||||
return ackRetryPSum;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct LOCO
|
struct LOCO
|
||||||
{
|
{
|
||||||
int loco;
|
int loco;
|
||||||
@@ -148,9 +99,10 @@ public:
|
|||||||
};
|
};
|
||||||
static LOCO speedTable[MAX_LOCOS];
|
static LOCO speedTable[MAX_LOCOS];
|
||||||
static int lookupSpeedTable(int locoId, bool autoCreate=true);
|
static int lookupSpeedTable(int locoId, bool autoCreate=true);
|
||||||
|
static byte cv1(byte opcode, int cv);
|
||||||
|
static byte cv2(int cv);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static byte joinRelay;
|
|
||||||
static byte loopStatus;
|
static byte loopStatus;
|
||||||
static void setThrottle2(uint16_t cab, uint8_t speedCode);
|
static void setThrottle2(uint16_t cab, uint8_t speedCode);
|
||||||
static void updateLocoReminder(int loco, byte speedCode);
|
static void updateLocoReminder(int loco, byte speedCode);
|
||||||
@@ -160,33 +112,9 @@ private:
|
|||||||
static FSH *shieldName;
|
static FSH *shieldName;
|
||||||
static byte globalSpeedsteps;
|
static byte globalSpeedsteps;
|
||||||
|
|
||||||
static byte cv1(byte opcode, int cv);
|
|
||||||
static byte cv2(int cv);
|
|
||||||
static void issueReminders();
|
static void issueReminders();
|
||||||
static void callback(int value);
|
static void callback(int value);
|
||||||
|
|
||||||
// ACK MANAGER
|
|
||||||
static ackOp const *ackManagerProg;
|
|
||||||
static ackOp const *ackManagerProgStart;
|
|
||||||
static byte ackManagerByte;
|
|
||||||
static byte ackManagerByteVerify;
|
|
||||||
static byte ackManagerBitNum;
|
|
||||||
static int ackManagerCv;
|
|
||||||
static byte ackManagerRetry;
|
|
||||||
static byte ackRetry;
|
|
||||||
static int16_t ackRetrySum;
|
|
||||||
static int16_t ackRetryPSum;
|
|
||||||
static int ackManagerWord;
|
|
||||||
static byte ackManagerStash;
|
|
||||||
static bool ackReceived;
|
|
||||||
static bool ackManagerRejoin;
|
|
||||||
static ACK_CALLBACK ackManagerCallback;
|
|
||||||
static CALLBACK_STATE callbackState;
|
|
||||||
static void ackManagerSetup(int cv, byte bitNumOrbyteValue, ackOp const program[], ACK_CALLBACK callback);
|
|
||||||
static void ackManagerSetup(int wordval, ackOp const program[], ACK_CALLBACK callback);
|
|
||||||
static void ackManagerLoop();
|
|
||||||
static bool checkResets( uint8_t numResets);
|
|
||||||
static const int PROG_REPEATS = 8; // repeats of programming commands (some decoders need at least 8 to be reliable)
|
|
||||||
|
|
||||||
// NMRA codes #
|
// NMRA codes #
|
||||||
static const byte SET_SPEED = 0x3f;
|
static const byte SET_SPEED = 0x3f;
|
||||||
@@ -201,31 +129,4 @@ private:
|
|||||||
static const byte BIT_OFF = 0x00;
|
static const byte BIT_OFF = 0x00;
|
||||||
};
|
};
|
||||||
|
|
||||||
#ifdef ARDUINO_AVR_MEGA // is using Mega 1280, define as Mega 2560 (pinouts and functionality are identical)
|
|
||||||
#define ARDUINO_AVR_MEGA2560
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(ARDUINO_AVR_UNO)
|
|
||||||
#define ARDUINO_TYPE "UNO"
|
|
||||||
#elif defined(ARDUINO_AVR_NANO)
|
|
||||||
#define ARDUINO_TYPE "NANO"
|
|
||||||
#elif defined(ARDUINO_AVR_MEGA2560)
|
|
||||||
#define ARDUINO_TYPE "MEGA"
|
|
||||||
#elif defined(ARDUINO_ARCH_MEGAAVR)
|
|
||||||
#define ARDUINO_TYPE "MEGAAVR"
|
|
||||||
#elif defined(ARDUINO_TEENSY32)
|
|
||||||
#define ARDUINO_TYPE "TEENSY32"
|
|
||||||
#elif defined(ARDUINO_TEENSY35)
|
|
||||||
#define ARDUINO_TYPE "TEENSY35"
|
|
||||||
#elif defined(ARDUINO_TEENSY36)
|
|
||||||
#define ARDUINO_TYPE "TEENSY36"
|
|
||||||
#elif defined(ARDUINO_TEENSY40)
|
|
||||||
#define ARDUINO_TYPE "TEENSY40"
|
|
||||||
#elif defined(ARDUINO_TEENSY41)
|
|
||||||
#define ARDUINO_TYPE "TEENSY41"
|
|
||||||
#else
|
|
||||||
#error CANNOT COMPILE - DCC++ EX ONLY WORKS WITH AN ARDUINO UNO, NANO 328, OR ARDUINO MEGA 1280/2560
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
468
DCCACK.cpp
Normal file
468
DCCACK.cpp
Normal file
@@ -0,0 +1,468 @@
|
|||||||
|
/*
|
||||||
|
* © 2021 M Steve Todd
|
||||||
|
* © 2021 Mike S
|
||||||
|
* © 2021 Fred Decker
|
||||||
|
* © 2020-2021 Harald Barth
|
||||||
|
* © 2020-2022 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/>.
|
||||||
|
*/
|
||||||
|
#include "DCCACK.h"
|
||||||
|
#include "DIAG.h"
|
||||||
|
#include "DCC.h"
|
||||||
|
#include "DCCWaveform.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
|
|
||||||
|
unsigned int DCCACK::minAckPulseDuration = 2000; // micros
|
||||||
|
unsigned int DCCACK::maxAckPulseDuration = 20000; // micros
|
||||||
|
|
||||||
|
MotorDriver * DCCACK::progDriver=NULL;
|
||||||
|
ackOp const * DCCACK::ackManagerProg;
|
||||||
|
ackOp const * DCCACK::ackManagerProgStart;
|
||||||
|
byte DCCACK::ackManagerByte;
|
||||||
|
byte DCCACK::ackManagerByteVerify;
|
||||||
|
byte DCCACK::ackManagerStash;
|
||||||
|
int DCCACK::ackManagerWord;
|
||||||
|
byte DCCACK::ackManagerRetry;
|
||||||
|
byte DCCACK::ackRetry = 2;
|
||||||
|
int16_t DCCACK::ackRetrySum;
|
||||||
|
int16_t DCCACK::ackRetryPSum;
|
||||||
|
int DCCACK::ackManagerCv;
|
||||||
|
byte DCCACK::ackManagerBitNum;
|
||||||
|
bool DCCACK::ackReceived;
|
||||||
|
bool DCCACK::ackManagerRejoin;
|
||||||
|
volatile uint8_t DCCACK::numAckGaps=0;
|
||||||
|
volatile uint8_t DCCACK::numAckSamples=0;
|
||||||
|
uint8_t DCCACK::trailingEdgeCounter=0;
|
||||||
|
|
||||||
|
|
||||||
|
unsigned int DCCACK::ackPulseDuration; // micros
|
||||||
|
unsigned long DCCACK::ackPulseStart; // micros
|
||||||
|
volatile bool DCCACK::ackDetected;
|
||||||
|
unsigned long DCCACK::ackCheckStart; // millis
|
||||||
|
volatile bool DCCACK::ackPending;
|
||||||
|
bool DCCACK::autoPowerOff;
|
||||||
|
int DCCACK::ackThreshold;
|
||||||
|
int DCCACK::ackLimitmA = 50;
|
||||||
|
int DCCACK::ackMaxCurrent;
|
||||||
|
unsigned int DCCACK::ackCheckDuration; // millis
|
||||||
|
|
||||||
|
|
||||||
|
CALLBACK_STATE DCCACK::callbackState=READY;
|
||||||
|
|
||||||
|
ACK_CALLBACK DCCACK::ackManagerCallback;
|
||||||
|
|
||||||
|
void DCCACK::Setup(int cv, byte byteValueOrBitnum, ackOp const program[], ACK_CALLBACK callback) {
|
||||||
|
progDriver=TrackManager::getProgDriver();
|
||||||
|
if (progDriver==NULL) {
|
||||||
|
callback(-3); // we dont have a prog track!
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (!progDriver->canMeasureCurrent()) {
|
||||||
|
callback(-2); // our prog track cant measure current
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
progDriver->setResetCounterPointer(&(DCCWaveform::progTrack.sentResetsSincePacket));
|
||||||
|
|
||||||
|
ackManagerRejoin=TrackManager::isJoined();
|
||||||
|
if (ackManagerRejoin ) {
|
||||||
|
// Change from JOIN must zero resets packet.
|
||||||
|
TrackManager::setJoin(false);
|
||||||
|
DCCWaveform::progTrack.sentResetsSincePacket = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
autoPowerOff=false;
|
||||||
|
if (progDriver->getPower() == POWERMODE::OFF) {
|
||||||
|
autoPowerOff=true; // power off afterwards
|
||||||
|
if (Diag::ACK) DIAG(F("Auto Prog power on"));
|
||||||
|
progDriver->setPower(POWERMODE::ON);
|
||||||
|
|
||||||
|
/* TODO !!! in MotorDriver surely!
|
||||||
|
if (MotorDriver::commonFaultPin)
|
||||||
|
DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON);
|
||||||
|
DCCWaveform::progTrack.sentResetsSincePacket = 0;
|
||||||
|
**/
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
ackManagerCv = cv;
|
||||||
|
ackManagerProg = program;
|
||||||
|
ackManagerProgStart = program;
|
||||||
|
ackManagerRetry = ackRetry;
|
||||||
|
ackManagerByte = byteValueOrBitnum;
|
||||||
|
ackManagerByteVerify = byteValueOrBitnum;
|
||||||
|
ackManagerBitNum=byteValueOrBitnum;
|
||||||
|
ackManagerCallback = callback;
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCACK::Setup(int wordval, ackOp const program[], ACK_CALLBACK callback) {
|
||||||
|
ackManagerWord=wordval;
|
||||||
|
Setup(0, 0, program, callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
const byte RESET_MIN=8; // tuning of reset counter before sending message
|
||||||
|
|
||||||
|
// checkRessets return true if the caller should yield back to loop and try later.
|
||||||
|
bool DCCACK::checkResets(uint8_t numResets) {
|
||||||
|
return DCCWaveform::progTrack.sentResetsSincePacket < numResets;
|
||||||
|
}
|
||||||
|
// Operations applicable to PROG track ONLY.
|
||||||
|
// (yes I know I could have subclassed the main track but...)
|
||||||
|
|
||||||
|
void DCCACK::setAckBaseline() {
|
||||||
|
int baseline=progDriver->getCurrentRaw();
|
||||||
|
ackThreshold= baseline + progDriver->mA2raw(ackLimitmA);
|
||||||
|
if (Diag::ACK) DIAG(F("ACK baseline=%d/%dmA Threshold=%d/%dmA Duration between %uus and %uus"),
|
||||||
|
baseline,progDriver->raw2mA(baseline),
|
||||||
|
ackThreshold,progDriver->raw2mA(ackThreshold),
|
||||||
|
minAckPulseDuration, maxAckPulseDuration);
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCACK::setAckPending() {
|
||||||
|
ackMaxCurrent=0;
|
||||||
|
ackPulseStart=0;
|
||||||
|
ackPulseDuration=0;
|
||||||
|
ackDetected=false;
|
||||||
|
ackCheckStart=millis();
|
||||||
|
numAckSamples=0;
|
||||||
|
numAckGaps=0;
|
||||||
|
ackPending=true; // interrupt routines will now take note
|
||||||
|
}
|
||||||
|
|
||||||
|
byte DCCACK::getAck() {
|
||||||
|
if (ackPending) return (2); // still waiting
|
||||||
|
if (Diag::ACK) DIAG(F("%S after %dmS max=%d/%dmA pulse=%uuS samples=%d gaps=%d"),ackDetected?F("ACK"):F("NO-ACK"), ackCheckDuration,
|
||||||
|
ackMaxCurrent,progDriver->raw2mA(ackMaxCurrent), ackPulseDuration, numAckSamples, numAckGaps);
|
||||||
|
if (ackDetected) return (1); // Yes we had an ack
|
||||||
|
return(0); // pending set off but not detected means no ACK.
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void DCCACK::loop() {
|
||||||
|
while (ackManagerProg) {
|
||||||
|
byte opcode=GETFLASH(ackManagerProg);
|
||||||
|
|
||||||
|
// breaks from this switch will step to next prog entry
|
||||||
|
// returns from this switch will stay on same entry
|
||||||
|
// (typically waiting for a reset counter or ACK waiting, or when all finished.)
|
||||||
|
switch (opcode) {
|
||||||
|
case BASELINE:
|
||||||
|
if (progDriver->getPower()==POWERMODE::OVERLOAD) return;
|
||||||
|
if (checkResets(autoPowerOff || ackManagerRejoin ? 20 : 3)) return;
|
||||||
|
setAckBaseline();
|
||||||
|
callbackState=AFTER_READ;
|
||||||
|
break;
|
||||||
|
case W0: // write 0 bit
|
||||||
|
case W1: // write 1 bit
|
||||||
|
{
|
||||||
|
if (checkResets(RESET_MIN)) return;
|
||||||
|
if (Diag::ACK) DIAG(F("W%d cv=%d bit=%d"),opcode==W1, ackManagerCv,ackManagerBitNum);
|
||||||
|
byte instruction = WRITE_BIT | (opcode==W1 ? BIT_ON : BIT_OFF) | ackManagerBitNum;
|
||||||
|
byte message[] = {DCC::cv1(BIT_MANIPULATE, ackManagerCv), DCC::cv2(ackManagerCv), instruction };
|
||||||
|
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
||||||
|
setAckPending();
|
||||||
|
callbackState=AFTER_WRITE;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case WB: // write byte
|
||||||
|
{
|
||||||
|
if (checkResets( RESET_MIN)) return;
|
||||||
|
if (Diag::ACK) DIAG(F("WB cv=%d value=%d"),ackManagerCv,ackManagerByte);
|
||||||
|
byte message[] = {DCC::cv1(WRITE_BYTE, ackManagerCv), DCC::cv2(ackManagerCv), ackManagerByte};
|
||||||
|
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
||||||
|
setAckPending();
|
||||||
|
callbackState=AFTER_WRITE;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case VB: // Issue validate Byte packet
|
||||||
|
{
|
||||||
|
if (checkResets( RESET_MIN)) return;
|
||||||
|
if (Diag::ACK) DIAG(F("VB cv=%d value=%d"),ackManagerCv,ackManagerByte);
|
||||||
|
byte message[] = { DCC::cv1(VERIFY_BYTE, ackManagerCv), DCC::cv2(ackManagerCv), ackManagerByte};
|
||||||
|
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
||||||
|
setAckPending();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case V0:
|
||||||
|
case V1: // Issue validate bit=0 or bit=1 packet
|
||||||
|
{
|
||||||
|
if (checkResets(RESET_MIN)) return;
|
||||||
|
if (Diag::ACK) DIAG(F("V%d cv=%d bit=%d"),opcode==V1, ackManagerCv,ackManagerBitNum);
|
||||||
|
byte instruction = VERIFY_BIT | (opcode==V0?BIT_OFF:BIT_ON) | ackManagerBitNum;
|
||||||
|
byte message[] = {DCC::cv1(BIT_MANIPULATE, ackManagerCv), DCC::cv2(ackManagerCv), instruction };
|
||||||
|
DCCWaveform::progTrack.schedulePacket(message, sizeof(message), PROG_REPEATS);
|
||||||
|
setAckPending();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case WACK: // wait for ack (or absence of ack)
|
||||||
|
{
|
||||||
|
byte ackState=2; // keep polling
|
||||||
|
|
||||||
|
ackState=getAck();
|
||||||
|
if (ackState==2) return; // keep polling
|
||||||
|
ackReceived=ackState==1;
|
||||||
|
break; // we have a genuine ACK result
|
||||||
|
}
|
||||||
|
case ITC0:
|
||||||
|
case ITC1: // If True Callback(0 or 1) (if prevous WACK got an ACK)
|
||||||
|
if (ackReceived) {
|
||||||
|
callback(opcode==ITC0?0:1);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ITCB: // If True callback(byte)
|
||||||
|
if (ackReceived) {
|
||||||
|
callback(ackManagerByte);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ITCBV: // If True callback(byte) - Verify
|
||||||
|
if (ackReceived) {
|
||||||
|
if (ackManagerByte == ackManagerByteVerify) {
|
||||||
|
ackRetrySum ++;
|
||||||
|
LCD(1, F("v %d %d Sum=%d"), ackManagerCv, ackManagerByte, ackRetrySum);
|
||||||
|
}
|
||||||
|
callback(ackManagerByte);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ITCB7: // If True callback(byte & 0x7F)
|
||||||
|
if (ackReceived) {
|
||||||
|
callback(ackManagerByte & 0x7F);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case NAKFAIL: // If nack callback(-1)
|
||||||
|
if (!ackReceived) {
|
||||||
|
callback(-1);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case CALLFAIL: // callback(-1)
|
||||||
|
callback(-1);
|
||||||
|
return;
|
||||||
|
|
||||||
|
case BIV: // ackManagerByte initial value
|
||||||
|
ackManagerByte = ackManagerByteVerify;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case STARTMERGE:
|
||||||
|
ackManagerBitNum=7;
|
||||||
|
ackManagerByte=0;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MERGE: // Merge previous Validate zero wack response with byte value and update bit number (use for reading CV bytes)
|
||||||
|
ackManagerByte <<= 1;
|
||||||
|
// ackReceived means bit is zero.
|
||||||
|
if (!ackReceived) ackManagerByte |= 1;
|
||||||
|
ackManagerBitNum--;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SETBIT:
|
||||||
|
ackManagerProg++;
|
||||||
|
ackManagerBitNum=GETFLASH(ackManagerProg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SETCV:
|
||||||
|
ackManagerProg++;
|
||||||
|
ackManagerCv=GETFLASH(ackManagerProg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SETBYTE:
|
||||||
|
ackManagerProg++;
|
||||||
|
ackManagerByte=GETFLASH(ackManagerProg);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SETBYTEH:
|
||||||
|
ackManagerByte=highByte(ackManagerWord);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SETBYTEL:
|
||||||
|
ackManagerByte=lowByte(ackManagerWord);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case STASHLOCOID:
|
||||||
|
ackManagerStash=ackManagerByte; // stash value from CV17
|
||||||
|
break;
|
||||||
|
|
||||||
|
case COMBINELOCOID:
|
||||||
|
// ackManagerStash is cv17, ackManagerByte is CV 18
|
||||||
|
callback( LONG_ADDR_MARKER | ( ackManagerByte + ((ackManagerStash - 192) << 8)));
|
||||||
|
return;
|
||||||
|
|
||||||
|
case ITSKIP:
|
||||||
|
if (!ackReceived) break;
|
||||||
|
// SKIP opcodes until SKIPTARGET found
|
||||||
|
while (opcode!=SKIPTARGET) {
|
||||||
|
ackManagerProg++;
|
||||||
|
opcode=GETFLASH(ackManagerProg);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case SKIPTARGET:
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
DIAG(F("!! ackOp %d FAULT!!"),opcode);
|
||||||
|
callback( -1);
|
||||||
|
return;
|
||||||
|
|
||||||
|
} // end of switch
|
||||||
|
ackManagerProg++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCACK::callback(int value) {
|
||||||
|
// check for automatic retry
|
||||||
|
if (value == -1 && ackManagerRetry > 0) {
|
||||||
|
ackRetrySum ++;
|
||||||
|
LCD(0, F("Retry %d %d Sum=%d"), ackManagerCv, ackManagerRetry, ackRetrySum);
|
||||||
|
ackManagerRetry --;
|
||||||
|
ackManagerProg = ackManagerProgStart;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
static unsigned long callbackStart;
|
||||||
|
// We are about to leave programming mode
|
||||||
|
// Rule 1: If we have written to a decoder we must maintain power for 100mS
|
||||||
|
// Rule 2: If we are re-joining the main track we must power off for 30mS
|
||||||
|
|
||||||
|
switch (callbackState) {
|
||||||
|
case AFTER_READ:
|
||||||
|
if (ackManagerRejoin && autoPowerOff) {
|
||||||
|
progDriver->setPower(POWERMODE::OFF);
|
||||||
|
callbackStart=millis();
|
||||||
|
callbackState=WAITING_30;
|
||||||
|
if (Diag::ACK) DIAG(F("OFF 30mS"));
|
||||||
|
} else {
|
||||||
|
callbackState=READY;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AFTER_WRITE: // first attempt to callback after a write operation
|
||||||
|
if (!ackManagerRejoin && !autoPowerOff) {
|
||||||
|
callbackState=READY;
|
||||||
|
break;
|
||||||
|
} // lines 906-910 added. avoid wait after write. use 1 PROG
|
||||||
|
callbackStart=millis();
|
||||||
|
callbackState=WAITING_100;
|
||||||
|
if (Diag::ACK) DIAG(F("Stable 100mS"));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case WAITING_100: // waiting for 100mS
|
||||||
|
if (millis()-callbackStart < 100) break;
|
||||||
|
// stable after power maintained for 100mS
|
||||||
|
|
||||||
|
// If we are going to power off anyway, it doesnt matter
|
||||||
|
// but if we will keep the power on, we must off it for 30mS
|
||||||
|
if (autoPowerOff) callbackState=READY;
|
||||||
|
else { // Need to cycle power off and on
|
||||||
|
progDriver->setPower(POWERMODE::OFF);
|
||||||
|
callbackStart=millis();
|
||||||
|
callbackState=WAITING_30;
|
||||||
|
if (Diag::ACK) DIAG(F("OFF 30mS"));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case WAITING_30: // waiting for 30mS with power off
|
||||||
|
if (millis()-callbackStart < 30) break;
|
||||||
|
//power has been off for 30mS
|
||||||
|
progDriver->setPower(POWERMODE::ON);
|
||||||
|
callbackState=READY;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case READY: // ready after read, or write after power delay and off period.
|
||||||
|
// power off if we powered it on
|
||||||
|
if (autoPowerOff) {
|
||||||
|
if (Diag::ACK) DIAG(F("Auto Prog power off"));
|
||||||
|
progDriver->setPower(POWERMODE::OFF);
|
||||||
|
/* TODO
|
||||||
|
if (MotorDriver::commonFaultPin)
|
||||||
|
DCCWaveform::mainTrack.setPowerMode(POWERMODE::OFF);
|
||||||
|
**/
|
||||||
|
}
|
||||||
|
// Restore <1 JOIN> to state before BASELINE
|
||||||
|
if (ackManagerRejoin) {
|
||||||
|
TrackManager::setJoin(true);
|
||||||
|
if (Diag::ACK) DIAG(F("Auto JOIN"));
|
||||||
|
}
|
||||||
|
|
||||||
|
ackManagerProg=NULL; // no more steps to execute
|
||||||
|
if (Diag::ACK) DIAG(F("Callback(%d)"),value);
|
||||||
|
(ackManagerCallback)( value);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void DCCACK::checkAck(byte sentResetsSincePacket) {
|
||||||
|
if (!ackPending) return;
|
||||||
|
// This function operates in interrupt() time so must be fast and can't DIAG
|
||||||
|
if (sentResetsSincePacket > 6) { //ACK timeout
|
||||||
|
ackCheckDuration=millis()-ackCheckStart;
|
||||||
|
ackPending = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int current=progDriver->getCurrentRawInInterrupt();
|
||||||
|
numAckSamples++;
|
||||||
|
if (current > ackMaxCurrent) ackMaxCurrent=current;
|
||||||
|
// An ACK is a pulse lasting between minAckPulseDuration and maxAckPulseDuration uSecs (refer @haba)
|
||||||
|
|
||||||
|
if (current>ackThreshold) {
|
||||||
|
if (trailingEdgeCounter > 0) {
|
||||||
|
numAckGaps++;
|
||||||
|
trailingEdgeCounter = 0;
|
||||||
|
}
|
||||||
|
if (ackPulseStart==0) ackPulseStart=micros(); // leading edge of pulse detected
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// not in pulse
|
||||||
|
if (ackPulseStart==0) return; // keep waiting for leading edge
|
||||||
|
|
||||||
|
// if we reach to this point, we have
|
||||||
|
// detected trailing edge of pulse
|
||||||
|
if (trailingEdgeCounter == 0) {
|
||||||
|
ackPulseDuration=micros()-ackPulseStart;
|
||||||
|
}
|
||||||
|
|
||||||
|
// but we do not trust it yet and return (which will force another
|
||||||
|
// measurement) and first the third time around with low current
|
||||||
|
// the ack detection will be finalized.
|
||||||
|
if (trailingEdgeCounter < 2) {
|
||||||
|
trailingEdgeCounter++;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
trailingEdgeCounter = 0;
|
||||||
|
|
||||||
|
if (ackPulseDuration>=minAckPulseDuration && ackPulseDuration<=maxAckPulseDuration) {
|
||||||
|
ackCheckDuration=millis()-ackCheckStart;
|
||||||
|
ackDetected=true;
|
||||||
|
ackPending=false;
|
||||||
|
DCCWaveform::progTrack.clearRepeats(); // shortcut remaining repeat packets
|
||||||
|
return; // we have a genuine ACK result
|
||||||
|
}
|
||||||
|
ackPulseStart=0; // We have detected a too-short or too-long pulse so ignore and wait for next leading edge
|
||||||
|
}
|
||||||
|
|
156
DCCACK.h
Normal file
156
DCCACK.h
Normal file
@@ -0,0 +1,156 @@
|
|||||||
|
/*
|
||||||
|
* © 2021 M Steve Todd
|
||||||
|
* © 2021 Mike S
|
||||||
|
* © 2021 Fred Decker
|
||||||
|
* © 2020-2021 Harald Barth
|
||||||
|
* © 2020-2022 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/>.
|
||||||
|
*/
|
||||||
|
#ifndef DCCACK_h
|
||||||
|
#define DCCACK_h
|
||||||
|
|
||||||
|
#include "MotorDriver.h"
|
||||||
|
|
||||||
|
typedef void (*ACK_CALLBACK)(int16_t result);
|
||||||
|
|
||||||
|
enum ackOp : byte
|
||||||
|
{ // Program opcodes for the ack Manager
|
||||||
|
BASELINE, // ensure enough resets sent before starting and obtain baseline current
|
||||||
|
W0,
|
||||||
|
W1, // issue write bit (0..1) packet
|
||||||
|
WB, // issue write byte packet
|
||||||
|
VB, // Issue validate Byte packet
|
||||||
|
V0, // Issue validate bit=0 packet
|
||||||
|
V1, // issue validate bit=1 packlet
|
||||||
|
WACK, // wait for ack (or absence of ack)
|
||||||
|
ITC1, // If True Callback(1) (if prevous WACK got an ACK)
|
||||||
|
ITC0, // If True callback(0);
|
||||||
|
ITCB, // If True callback(byte)
|
||||||
|
ITCBV, // If True callback(byte) - end of Verify Byte
|
||||||
|
ITCB7, // If True callback(byte &0x7F)
|
||||||
|
NAKFAIL, // if false callback(-1)
|
||||||
|
CALLFAIL, // callback(-1)
|
||||||
|
BIV, // Set ackManagerByte to initial value for Verify retry
|
||||||
|
STARTMERGE, // Clear bit and byte settings ready for merge pass
|
||||||
|
MERGE, // Merge previous wack response with byte value and decrement bit number (use for readimng CV bytes)
|
||||||
|
SETBIT, // sets bit number to next prog byte
|
||||||
|
SETCV, // sets cv number to next prog byte
|
||||||
|
SETBYTE, // sets current byte to next prog byte
|
||||||
|
SETBYTEH, // sets current byte to word high byte
|
||||||
|
SETBYTEL, // sets current byte to word low byte
|
||||||
|
STASHLOCOID, // keeps current byte value for later
|
||||||
|
COMBINELOCOID, // combines current value with stashed value and returns it
|
||||||
|
ITSKIP, // skip to SKIPTARGET if ack true
|
||||||
|
SKIPTARGET = 0xFF // jump to target
|
||||||
|
};
|
||||||
|
|
||||||
|
enum CALLBACK_STATE : byte {
|
||||||
|
|
||||||
|
AFTER_READ, // Start callback sequence after something was read from the decoder
|
||||||
|
AFTER_WRITE, // Start callback sequence after something was written to the decoder
|
||||||
|
WAITING_100, // Waiting for 100mS of stable power
|
||||||
|
WAITING_30, // waiting to 30ms of power off gap.
|
||||||
|
READY, // Ready to complete callback
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class DCCACK {
|
||||||
|
public:
|
||||||
|
static byte getAck(); //prog track only 0=NACK, 1=ACK 2=keep waiting
|
||||||
|
static void checkAck(byte sentResetsSincePacket); // Interrupt time ack checker
|
||||||
|
static inline void setAckLimit(int mA) {
|
||||||
|
ackLimitmA = mA;
|
||||||
|
}
|
||||||
|
static inline void setMinAckPulseDuration(unsigned int i) {
|
||||||
|
minAckPulseDuration = i;
|
||||||
|
}
|
||||||
|
static inline void setMaxAckPulseDuration(unsigned int i) {
|
||||||
|
maxAckPulseDuration = i;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void Setup(int cv, byte byteValueOrBitnum, ackOp const program[], ACK_CALLBACK callback);
|
||||||
|
static void Setup(int wordval, ackOp const program[], ACK_CALLBACK callback);
|
||||||
|
static void loop();
|
||||||
|
static bool isActive() { return ackManagerProg!=NULL;}
|
||||||
|
static inline int16_t setAckRetry(byte retry) {
|
||||||
|
ackRetry = retry;
|
||||||
|
ackRetryPSum = ackRetrySum;
|
||||||
|
ackRetrySum = 0; // reset running total
|
||||||
|
return ackRetryPSum;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
private:
|
||||||
|
static const byte SET_SPEED = 0x3f;
|
||||||
|
static const byte WRITE_BYTE = 0x7C;
|
||||||
|
static const byte VERIFY_BYTE = 0x74;
|
||||||
|
static const byte BIT_MANIPULATE = 0x78;
|
||||||
|
static const byte WRITE_BIT = 0xF0;
|
||||||
|
static const byte VERIFY_BIT = 0xE0;
|
||||||
|
static const byte BIT_ON = 0x08;
|
||||||
|
static const byte BIT_OFF = 0x00;
|
||||||
|
|
||||||
|
static void setAckBaseline();
|
||||||
|
static void setAckPending();
|
||||||
|
static void callback(int value);
|
||||||
|
|
||||||
|
static const int PROG_REPEATS = 8; // repeats of programming commands (some decoders need at least 8 to be reliable)
|
||||||
|
|
||||||
|
// ACK management (Prog track only)
|
||||||
|
static void checkAck();
|
||||||
|
static bool checkResets(uint8_t numResets);
|
||||||
|
|
||||||
|
static volatile bool ackPending;
|
||||||
|
static volatile bool ackDetected;
|
||||||
|
static int ackThreshold;
|
||||||
|
static int ackLimitmA;
|
||||||
|
static int ackMaxCurrent;
|
||||||
|
static unsigned long ackCheckStart; // millis
|
||||||
|
static unsigned int ackCheckDuration; // millis
|
||||||
|
|
||||||
|
static unsigned int ackPulseDuration; // micros
|
||||||
|
static unsigned long ackPulseStart; // micros
|
||||||
|
|
||||||
|
static unsigned int minAckPulseDuration ; // micros
|
||||||
|
static unsigned int maxAckPulseDuration ; // micros
|
||||||
|
static MotorDriver* progDriver;
|
||||||
|
static volatile uint8_t numAckGaps;
|
||||||
|
static volatile uint8_t numAckSamples;
|
||||||
|
static uint8_t trailingEdgeCounter;
|
||||||
|
static ackOp const * ackManagerProg;
|
||||||
|
static ackOp const * ackManagerProgStart;
|
||||||
|
static byte ackManagerByte;
|
||||||
|
static byte ackManagerByteVerify;
|
||||||
|
static byte ackManagerStash;
|
||||||
|
static int ackManagerWord;
|
||||||
|
static byte ackManagerRetry;
|
||||||
|
static byte ackRetry;
|
||||||
|
static int16_t ackRetrySum;
|
||||||
|
static int16_t ackRetryPSum;
|
||||||
|
static int ackManagerCv;
|
||||||
|
static byte ackManagerBitNum;
|
||||||
|
static bool ackReceived;
|
||||||
|
static bool ackManagerRejoin;
|
||||||
|
static bool autoPowerOff;
|
||||||
|
static CALLBACK_STATE callbackState;
|
||||||
|
static ACK_CALLBACK ackManagerCallback;
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
#endif
|
3
DCCEX.h
3
DCCEX.h
@@ -38,12 +38,13 @@
|
|||||||
#endif
|
#endif
|
||||||
#include "LCD_Implementation.h"
|
#include "LCD_Implementation.h"
|
||||||
#include "LCN.h"
|
#include "LCN.h"
|
||||||
#include "freeMemory.h"
|
|
||||||
#include "IODevice.h"
|
#include "IODevice.h"
|
||||||
#include "Turnouts.h"
|
#include "Turnouts.h"
|
||||||
#include "Sensors.h"
|
#include "Sensors.h"
|
||||||
#include "Outputs.h"
|
#include "Outputs.h"
|
||||||
#include "CommandDistributor.h"
|
#include "CommandDistributor.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
|
#include "DCCTimer.h"
|
||||||
#include "EXRAIL.h"
|
#include "EXRAIL.h"
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@@ -30,26 +30,20 @@
|
|||||||
#include "Turnouts.h"
|
#include "Turnouts.h"
|
||||||
#include "Outputs.h"
|
#include "Outputs.h"
|
||||||
#include "Sensors.h"
|
#include "Sensors.h"
|
||||||
#include "freeMemory.h"
|
|
||||||
#include "GITHUB_SHA.h"
|
#include "GITHUB_SHA.h"
|
||||||
#include "version.h"
|
#include "version.h"
|
||||||
#include "defines.h"
|
#include "defines.h"
|
||||||
#include "CommandDistributor.h"
|
#include "CommandDistributor.h"
|
||||||
#include "EEStore.h"
|
#include "EEStore.h"
|
||||||
#include "DIAG.h"
|
#include "DIAG.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
|
#include "DCCTimer.h"
|
||||||
#include "EXRAIL2.h"
|
#include "EXRAIL2.h"
|
||||||
|
#ifdef HAS_AVR_WDT
|
||||||
#include <avr/wdt.h>
|
#include <avr/wdt.h>
|
||||||
|
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
|
||||||
//
|
|
||||||
// Figure out if we have enough memory for advanced features
|
|
||||||
//
|
|
||||||
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_NANO)
|
|
||||||
// nope
|
|
||||||
#else
|
|
||||||
#define HAS_ENOUGH_MEMORY
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
// These keywords are used in the <1> command. The number is what you get if you use the keyword as a parameter.
|
// 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
|
// To discover new keyword numbers , use the <$ YOURKEYWORD> command
|
||||||
const int16_t HASH_KEYWORD_PROG = -29718;
|
const int16_t HASH_KEYWORD_PROG = -29718;
|
||||||
@@ -278,20 +272,29 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
return;
|
return;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'a': // ACCESSORY <a ADDRESS SUBADDRESS ACTIVATE> or <a LINEARADDRESS ACTIVATE>
|
case 'a': // ACCESSORY <a ADDRESS SUBADDRESS ACTIVATE [ONOFF]> or <a LINEARADDRESS ACTIVATE>
|
||||||
{
|
{
|
||||||
int address;
|
int address;
|
||||||
byte subaddress;
|
byte subaddress;
|
||||||
byte activep;
|
byte activep;
|
||||||
|
byte onoff;
|
||||||
if (params==2) { // <a LINEARADDRESS ACTIVATE>
|
if (params==2) { // <a LINEARADDRESS ACTIVATE>
|
||||||
address=(p[0] - 1) / 4 + 1;
|
address=(p[0] - 1) / 4 + 1;
|
||||||
subaddress=(p[0] - 1) % 4;
|
subaddress=(p[0] - 1) % 4;
|
||||||
activep=1;
|
activep=1;
|
||||||
|
onoff=2; // send both
|
||||||
}
|
}
|
||||||
else if (params==3) { // <a ADDRESS SUBADDRESS ACTIVATE>
|
else if (params==3) { // <a ADDRESS SUBADDRESS ACTIVATE>
|
||||||
address=p[0];
|
address=p[0];
|
||||||
subaddress=p[1];
|
subaddress=p[1];
|
||||||
activep=2;
|
activep=2;
|
||||||
|
onoff=2; // send both
|
||||||
|
}
|
||||||
|
else if (params==4) { // <a ADDRESS SUBADDRESS ACTIVATE ONOFF>
|
||||||
|
address=p[0];
|
||||||
|
subaddress=p[1];
|
||||||
|
activep=2;
|
||||||
|
onoff=p[3];
|
||||||
}
|
}
|
||||||
else break; // invalid no of parameters
|
else break; // invalid no of parameters
|
||||||
|
|
||||||
@@ -299,12 +302,13 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
((address & 0x01FF) != address) // invalid address (limit 9 bits )
|
((address & 0x01FF) != address) // invalid address (limit 9 bits )
|
||||||
|| ((subaddress & 0x03) != subaddress) // invalid subaddress (limit 2 bits )
|
|| ((subaddress & 0x03) != subaddress) // invalid subaddress (limit 2 bits )
|
||||||
|| ((p[activep] & 0x01) != p[activep]) // invalid activate 0|1
|
|| ((p[activep] & 0x01) != p[activep]) // invalid activate 0|1
|
||||||
|
|| ((onoff & 0x01) != onoff) // invalid onoff 0|1
|
||||||
) break;
|
) break;
|
||||||
// Honour the configuration option (config.h) which allows the <a> command to be reversed
|
// Honour the configuration option (config.h) which allows the <a> command to be reversed
|
||||||
#ifdef DCC_ACCESSORY_COMMAND_REVERSE
|
#ifdef DCC_ACCESSORY_COMMAND_REVERSE
|
||||||
DCC::setAccessory(address, subaddress,p[activep]==0);
|
DCC::setAccessory(address, subaddress,p[activep]==0,onoff);
|
||||||
#else
|
#else
|
||||||
DCC::setAccessory(address, subaddress,p[activep]==1);
|
DCC::setAccessory(address, subaddress,p[activep]==1,onoff);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
@@ -386,7 +390,7 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
{ // <R CV> -- uses verify callback
|
{ // <R CV> -- uses verify callback
|
||||||
if (!stashCallback(stream, p, ringStream))
|
if (!stashCallback(stream, p, ringStream))
|
||||||
break;
|
break;
|
||||||
DCC::verifyCVByte(p[0], p[1], callback_Vbyte);
|
DCC::verifyCVByte(p[0], 0, callback_Vbyte);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (params == 3)
|
if (params == 3)
|
||||||
@@ -429,9 +433,9 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
}
|
}
|
||||||
else break; // will reply <X>
|
else break; // will reply <X>
|
||||||
}
|
}
|
||||||
if (main) DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON);
|
if (main) TrackManager::setMainPower(POWERMODE::ON);
|
||||||
if (prog) DCCWaveform::progTrack.setPowerMode(POWERMODE::ON);
|
if (prog) TrackManager::setProgPower(POWERMODE::ON);
|
||||||
DCC::setProgTrackSyncMain(join);
|
TrackManager::setJoin(join);
|
||||||
|
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
return;
|
return;
|
||||||
@@ -456,12 +460,12 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
else break; // will reply <X>
|
else break; // will reply <X>
|
||||||
}
|
}
|
||||||
|
|
||||||
if (main) DCCWaveform::mainTrack.setPowerMode(POWERMODE::OFF);
|
if (main) TrackManager::setMainPower(POWERMODE::OFF);
|
||||||
if (prog) {
|
if (prog) {
|
||||||
DCC::setProgTrackBoost(false); // Prog track boost mode will not outlive prog track off
|
TrackManager::progTrackBoosted=false; // Prog track boost mode will not outlive prog track off
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::OFF);
|
TrackManager::setProgPower(POWERMODE::OFF);
|
||||||
}
|
}
|
||||||
DCC::setProgTrackSyncMain(false);
|
TrackManager::setJoin(false);
|
||||||
|
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
return;
|
return;
|
||||||
@@ -472,18 +476,14 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
return;
|
return;
|
||||||
|
|
||||||
case 'c': // SEND METER RESPONSES <c>
|
case 'c': // SEND METER RESPONSES <c>
|
||||||
// <c MeterName value C/V unit min max res warn>
|
// No longer supported because of multiple tracks <c MeterName value C/V unit min max res warn>
|
||||||
StringFormatter::send(stream, F("<c CurrentMAIN %d C Milli 0 %d 1 %d>\n"), DCCWaveform::mainTrack.getCurrentmA(),
|
break;
|
||||||
DCCWaveform::mainTrack.getMaxmA(), DCCWaveform::mainTrack.getTripmA());
|
|
||||||
StringFormatter::send(stream, F("<a %d>\n"), DCCWaveform::mainTrack.get1024Current()); //'a' message deprecated, remove once JMRI 4.22 is available
|
|
||||||
return;
|
|
||||||
|
|
||||||
case 'Q': // SENSORS <Q>
|
case 'Q': // SENSORS <Q>
|
||||||
Sensor::printAll(stream);
|
Sensor::printAll(stream);
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case 's': // <s>
|
case 's': // <s>
|
||||||
StringFormatter::send(stream, F("<p%d>\n"), DCCWaveform::mainTrack.getPowerMode() == POWERMODE::ON);
|
|
||||||
StringFormatter::send(stream, F("<iDCC-EX V-%S / %S / %S G-%S>\n"), F(VERSION), F(ARDUINO_TYPE), DCC::getMotorShieldName(), F(GITHUB_SHA));
|
StringFormatter::send(stream, F("<iDCC-EX V-%S / %S / %S G-%S>\n"), F(VERSION), F(ARDUINO_TYPE), DCC::getMotorShieldName(), F(GITHUB_SHA));
|
||||||
Turnout::printAll(stream); //send all Turnout states
|
Turnout::printAll(stream); //send all Turnout states
|
||||||
Output::printAll(stream); //send all Output states
|
Output::printAll(stream); //send all Output states
|
||||||
@@ -511,6 +511,11 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
return;
|
return;
|
||||||
return;
|
return;
|
||||||
|
|
||||||
|
case '=': // <= Track manager control >
|
||||||
|
if (TrackManager::parseJ(stream, params, p))
|
||||||
|
return;
|
||||||
|
break;
|
||||||
|
|
||||||
case '#': // NUMBER OF LOCOSLOTS <#>
|
case '#': // NUMBER OF LOCOSLOTS <#>
|
||||||
StringFormatter::send(stream, F("<# %d>\n"), MAX_LOCOS);
|
StringFormatter::send(stream, F("<# %d>\n"), MAX_LOCOS);
|
||||||
return;
|
return;
|
||||||
@@ -531,8 +536,7 @@ void DCCEXParser::parse(Print *stream, byte *com, RingStream * ringStream)
|
|||||||
#if WIFI_ON
|
#if WIFI_ON
|
||||||
case '+': // Complex Wifi interface command (not usual parse)
|
case '+': // Complex Wifi interface command (not usual parse)
|
||||||
if (atCommandCallback && !ringStream) {
|
if (atCommandCallback && !ringStream) {
|
||||||
DCCWaveform::mainTrack.setPowerMode(POWERMODE::OFF);
|
TrackManager::setPower(POWERMODE::OFF);
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::OFF);
|
|
||||||
atCommandCallback((HardwareSerial *)stream,com);
|
atCommandCallback((HardwareSerial *)stream,com);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@@ -831,23 +835,23 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
|
|||||||
return true;
|
return true;
|
||||||
|
|
||||||
case HASH_KEYWORD_RAM: // <D RAM>
|
case HASH_KEYWORD_RAM: // <D RAM>
|
||||||
StringFormatter::send(stream, F("Free memory=%d\n"), minimumFreeMemory());
|
StringFormatter::send(stream, F("Free memory=%d\n"), DCCTimer::getMinimumFreeMemory());
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case HASH_KEYWORD_ACK: // <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 (params >= 3) {
|
||||||
if (p[1] == HASH_KEYWORD_LIMIT) {
|
if (p[1] == HASH_KEYWORD_LIMIT) {
|
||||||
DCCWaveform::progTrack.setAckLimit(p[2]);
|
DCCACK::setAckLimit(p[2]);
|
||||||
LCD(1, F("Ack Limit=%dmA"), p[2]); // <D ACK LIMIT 42>
|
LCD(1, F("Ack Limit=%dmA"), p[2]); // <D ACK LIMIT 42>
|
||||||
} else if (p[1] == HASH_KEYWORD_MIN) {
|
} else if (p[1] == HASH_KEYWORD_MIN) {
|
||||||
DCCWaveform::progTrack.setMinAckPulseDuration(p[2]);
|
DCCACK::setMinAckPulseDuration(p[2]);
|
||||||
LCD(0, F("Ack Min=%uus"), p[2]); // <D ACK MIN 1500>
|
LCD(0, F("Ack Min=%uus"), p[2]); // <D ACK MIN 1500>
|
||||||
} else if (p[1] == HASH_KEYWORD_MAX) {
|
} else if (p[1] == HASH_KEYWORD_MAX) {
|
||||||
DCCWaveform::progTrack.setMaxAckPulseDuration(p[2]);
|
DCCACK::setMaxAckPulseDuration(p[2]);
|
||||||
LCD(0, F("Ack Max=%uus"), p[2]); // <D ACK MAX 9000>
|
LCD(0, F("Ack Max=%uus"), p[2]); // <D ACK MAX 9000>
|
||||||
} else if (p[1] == HASH_KEYWORD_RETRY) {
|
} else if (p[1] == HASH_KEYWORD_RETRY) {
|
||||||
if (p[2] >255) p[2]=3;
|
if (p[2] >255) p[2]=3;
|
||||||
LCD(0, F("Ack Retry=%d Sum=%d"), p[2], DCC::setAckRetry(p[2])); // <D ACK RETRY 2>
|
LCD(0, F("Ack Retry=%d Sum=%d"), p[2], DCCACK::setAckRetry(p[2])); // <D ACK RETRY 2>
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
StringFormatter::send(stream, F("Ack diag %S\n"), onOff ? F("on") : F("off"));
|
StringFormatter::send(stream, F("Ack diag %S\n"), onOff ? F("on") : F("off"));
|
||||||
@@ -878,13 +882,17 @@ bool DCCEXParser::parseD(Print *stream, int16_t params, int16_t p[])
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
case HASH_KEYWORD_PROGBOOST:
|
case HASH_KEYWORD_PROGBOOST:
|
||||||
DCC::setProgTrackBoost(true);
|
TrackManager::progTrackBoosted=true;
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
case HASH_KEYWORD_RESET:
|
case HASH_KEYWORD_RESET:
|
||||||
{
|
{
|
||||||
|
#ifdef HAS_AVR_WDT
|
||||||
wdt_enable( WDTO_15MS); // set Arduino watchdog timer for 15ms
|
wdt_enable( WDTO_15MS); // set Arduino watchdog timer for 15ms
|
||||||
delay(50); // wait for the prescaller time to expire
|
delay(50); // wait for the prescaller time to expire
|
||||||
|
#else
|
||||||
|
ESP.restart();
|
||||||
|
#endif
|
||||||
break; // and <X> if we didnt restart
|
break; // and <X> if we didnt restart
|
||||||
}
|
}
|
||||||
|
|
||||||
|
95
DCCTimer.h
95
DCCTimer.h
@@ -20,6 +20,34 @@
|
|||||||
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
/* There are several different implementations of this class which the compiler will select
|
||||||
|
according to the hardware.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* This timer class is used to manage the single timer required to handle the DCC waveform.
|
||||||
|
* All timer access comes through this class so that it can be compiled for
|
||||||
|
* various hardware CPU types.
|
||||||
|
*
|
||||||
|
* DCCEX works on a single timer interrupt at a regular 58uS interval.
|
||||||
|
* The DCCWaveform class generates the signals to the motor shield
|
||||||
|
* based on this timer.
|
||||||
|
*
|
||||||
|
* If the motor drivers are BOTH configured to use the correct 2 pins for the architecture,
|
||||||
|
* (see isPWMPin() function. )
|
||||||
|
* then this allows us to use a hardware driven pin switching arrangement which is
|
||||||
|
* achieved by setting the duty cycle of the NEXT clock interrupt to 0% or 100% depending on
|
||||||
|
* the required pin state. (see setPWM())
|
||||||
|
* This is more accurate than the software interrupt but at the expense of
|
||||||
|
* limiting the choice of available pins.
|
||||||
|
* Fortunately, a standard motor shield on a Mega uses pins that qualify for PWM...
|
||||||
|
* Other shields may be jumpered to PWM pins or run directly using the software interrupt.
|
||||||
|
*
|
||||||
|
* Because the PWM-based waveform is effectively set half a cycle after the software version,
|
||||||
|
* it is not acceptable to drive the two tracks on different methiods or it would cause
|
||||||
|
* problems for <1 JOIN> etc.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
#ifndef DCCTimer_h
|
#ifndef DCCTimer_h
|
||||||
#define DCCTimer_h
|
#define DCCTimer_h
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
@@ -32,11 +60,68 @@ class DCCTimer {
|
|||||||
static void getSimulatedMacAddress(byte mac[6]);
|
static void getSimulatedMacAddress(byte mac[6]);
|
||||||
static bool isPWMPin(byte pin);
|
static bool isPWMPin(byte pin);
|
||||||
static void setPWM(byte pin, bool high);
|
static void setPWM(byte pin, bool high);
|
||||||
#if (defined(TEENSYDUINO) && !defined(__IMXRT1062__))
|
static void clearPWM();
|
||||||
static void read_mac(byte mac[6]);
|
// Update low ram level. Allow for extra bytes to be specified
|
||||||
static void read(uint8_t word, uint8_t *mac, uint8_t offset);
|
// by estimation or inspection, that may be used by other
|
||||||
#endif
|
// called subroutines. Must be called with interrupts disabled.
|
||||||
private:
|
//
|
||||||
|
// Although __brkval may go up and down as heap memory is allocated
|
||||||
|
// and freed, this function records only the worst case encountered.
|
||||||
|
// So even if all of the heap is freed, the reported minimum free
|
||||||
|
// memory will not increase.
|
||||||
|
//
|
||||||
|
static void inline updateMinimumFreeMemoryISR(unsigned char extraBytes=0) {
|
||||||
|
int spare = freeMemory()-extraBytes;
|
||||||
|
if (spare < 0) spare = 0;
|
||||||
|
if (spare < minimum_free_memory) minimum_free_memory = spare;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int getMinimumFreeMemory();
|
||||||
|
|
||||||
|
private:
|
||||||
|
static int freeMemory();
|
||||||
|
static volatile int minimum_free_memory;
|
||||||
|
static const int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle
|
||||||
|
static const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME) >>1;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
|
// Create a cpu type we can share and
|
||||||
|
// gigure out if we have enough memory for advanced features
|
||||||
|
// so define HAS_ENOUGH_MEMORY until proved otherwise.
|
||||||
|
#define HAS_ENOUGH_MEMORY
|
||||||
|
#define HAS_AVR_WDT
|
||||||
|
|
||||||
|
#if defined(ARDUINO_AVR_UNO)
|
||||||
|
#define ARDUINO_TYPE "UNO"
|
||||||
|
#undef HAS_ENOUGH_MEMORY
|
||||||
|
#elif defined(ARDUINO_AVR_NANO)
|
||||||
|
#define ARDUINO_TYPE "NANO"
|
||||||
|
#undef HAS_ENOUGH_MEMORY
|
||||||
|
#elif defined(ARDUINO_AVR_MEGA)
|
||||||
|
#define ARDUINO_TYPE "MEGA"
|
||||||
|
#elif defined(ARDUINO_AVR_MEGA2560)
|
||||||
|
#define ARDUINO_TYPE "MEGA"
|
||||||
|
#elif defined(ARDUINO_ARCH_MEGAAVR)
|
||||||
|
#define ARDUINO_TYPE "MEGAAVR"
|
||||||
|
#elif defined(ARDUINO_TEENSY32)
|
||||||
|
#define ARDUINO_TYPE "TEENSY32"
|
||||||
|
#elif defined(ARDUINO_TEENSY35)
|
||||||
|
#define ARDUINO_TYPE "TEENSY35"
|
||||||
|
#elif defined(ARDUINO_TEENSY36)
|
||||||
|
#define ARDUINO_TYPE "TEENSY36"
|
||||||
|
#elif defined(ARDUINO_TEENSY40)
|
||||||
|
#define ARDUINO_TYPE "TEENSY40"
|
||||||
|
#elif defined(ARDUINO_TEENSY41)
|
||||||
|
#define ARDUINO_TYPE "TEENSY41"
|
||||||
|
#elif defined(ARDUINO_ARCH_ESP8266)
|
||||||
|
#define ARDUINO_TYPE "ESP8266"
|
||||||
|
#undef HAS_AVR_WDT
|
||||||
|
#elif defined(ARDUINO_ARCH_ESP32)
|
||||||
|
#define ARDUINO_TYPE "ESP32"
|
||||||
|
#undef HAS_AVR_WDT
|
||||||
|
#else
|
||||||
|
#error CANNOT COMPILE - DCC++ EX ONLY WORKS WITH THE ARCHITECTURES LISTED IN DCCTimer.h
|
||||||
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
117
DCCTimerAVR.cpp
Normal file
117
DCCTimerAVR.cpp
Normal file
@@ -0,0 +1,117 @@
|
|||||||
|
/*
|
||||||
|
* © 2021 Mike S
|
||||||
|
* © 2021 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 UNO or MEGA
|
||||||
|
// Please refer to DCCTimer.h for general comments about how this class works
|
||||||
|
// This is to avoid repetition and duplication.
|
||||||
|
#ifdef ARDUINO_ARCH_AVR
|
||||||
|
|
||||||
|
#include <avr/boot.h>
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
INTERRUPT_CALLBACK interruptHandler=0;
|
||||||
|
|
||||||
|
// Arduino nano, uno, mega etc
|
||||||
|
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
||||||
|
#define TIMER1_A_PIN 11
|
||||||
|
#define TIMER1_B_PIN 12
|
||||||
|
#define TIMER1_C_PIN 13
|
||||||
|
#else
|
||||||
|
#define TIMER1_A_PIN 9
|
||||||
|
#define TIMER1_B_PIN 10
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||||
|
interruptHandler=callback;
|
||||||
|
noInterrupts();
|
||||||
|
ADCSRA = (ADCSRA & 0b11111000) | 0b00000100; // speed up analogRead sample time
|
||||||
|
TCCR1A = 0;
|
||||||
|
ICR1 = CLOCK_CYCLES;
|
||||||
|
TCNT1 = 0;
|
||||||
|
TCCR1B = _BV(WGM13) | _BV(CS10); // Mode 8, clock select 1
|
||||||
|
TIMSK1 = _BV(TOIE1); // Enable Software interrupt
|
||||||
|
interrupts();
|
||||||
|
}
|
||||||
|
|
||||||
|
// ISR called by timer interrupt every 58uS
|
||||||
|
ISR(TIMER1_OVF_vect){ interruptHandler(); }
|
||||||
|
|
||||||
|
// Alternative pin manipulation via PWM control.
|
||||||
|
bool DCCTimer::isPWMPin(byte pin) {
|
||||||
|
return pin==TIMER1_A_PIN
|
||||||
|
|| pin==TIMER1_B_PIN
|
||||||
|
#ifdef TIMER1_C_PIN
|
||||||
|
|| pin==TIMER1_C_PIN
|
||||||
|
#endif
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCTimer::setPWM(byte pin, bool high) {
|
||||||
|
if (pin==TIMER1_A_PIN) {
|
||||||
|
TCCR1A |= _BV(COM1A1);
|
||||||
|
OCR1A= high?1024:0;
|
||||||
|
}
|
||||||
|
else if (pin==TIMER1_B_PIN) {
|
||||||
|
TCCR1A |= _BV(COM1B1);
|
||||||
|
OCR1B= high?1024:0;
|
||||||
|
}
|
||||||
|
#ifdef TIMER1_C_PIN
|
||||||
|
else if (pin==TIMER1_C_PIN) {
|
||||||
|
TCCR1A |= _BV(COM1C1);
|
||||||
|
OCR1C= high?1024:0;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCTimer::clearPWM() {
|
||||||
|
TCCR1A= 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||||
|
for (byte i=0; i<6; i++) {
|
||||||
|
mac[i]=boot_signature_byte_get(0x0E + i);
|
||||||
|
}
|
||||||
|
mac[0] &= 0xFE;
|
||||||
|
mac[0] |= 0x02;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
|
||||||
|
|
||||||
|
// Return low memory value...
|
||||||
|
int DCCTimer::getMinimumFreeMemory() {
|
||||||
|
noInterrupts(); // Disable interrupts to get volatile value
|
||||||
|
int retval = minimum_free_memory;
|
||||||
|
interrupts();
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
extern char *__brkval;
|
||||||
|
extern char *__malloc_heap_start;
|
||||||
|
|
||||||
|
int DCCTimer::freeMemory() {
|
||||||
|
char top;
|
||||||
|
return __brkval ? &top - __brkval : &top - __malloc_heap_start;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
129
DCCTimerESP.cpp
Normal file
129
DCCTimerESP.cpp
Normal file
@@ -0,0 +1,129 @@
|
|||||||
|
/*
|
||||||
|
* © 2020-2022 Harald Barth
|
||||||
|
*
|
||||||
|
* 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/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
// ATTENTION: this file only compiles on an ESP8266 and ESP32
|
||||||
|
// On ESP32 we do not even use the functions but they are here for completeness sake
|
||||||
|
// Please refer to DCCTimer.h for general comments about how this class works
|
||||||
|
// This is to avoid repetition and duplication.
|
||||||
|
|
||||||
|
#ifdef ARDUINO_ARCH_ESP8266
|
||||||
|
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
INTERRUPT_CALLBACK interruptHandler=0;
|
||||||
|
|
||||||
|
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||||
|
interruptHandler=callback;
|
||||||
|
timer1_disable();
|
||||||
|
|
||||||
|
// There seem to be differnt ways to attach interrupt handler
|
||||||
|
// ETS_FRC_TIMER1_INTR_ATTACH(NULL, NULL);
|
||||||
|
// ETS_FRC_TIMER1_NMI_INTR_ATTACH(interruptHandler);
|
||||||
|
// Let us choose the one from the API
|
||||||
|
timer1_attachInterrupt(interruptHandler);
|
||||||
|
|
||||||
|
// not exactly sure of order:
|
||||||
|
timer1_enable(TIM_DIV1, TIM_EDGE, TIM_LOOP);
|
||||||
|
timer1_write(CLOCK_CYCLES);
|
||||||
|
}
|
||||||
|
// We do not support to use PWM to make the Waveform on ESP
|
||||||
|
bool IRAM_ATTR DCCTimer::isPWMPin(byte pin) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
void IRAM_ATTR DCCTimer::setPWM(byte pin, bool high) {
|
||||||
|
}
|
||||||
|
void IRAM_ATTR DCCTimer::clearPWM() {
|
||||||
|
}
|
||||||
|
|
||||||
|
// Fake this as it should not be used
|
||||||
|
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||||
|
mac[0] = 0xFE;
|
||||||
|
mac[1] = 0xBE;
|
||||||
|
mac[2] = 0xEF;
|
||||||
|
mac[3] = 0xC0;
|
||||||
|
mac[4] = 0xFF;
|
||||||
|
mac[5] = 0xEE;
|
||||||
|
}
|
||||||
|
|
||||||
|
volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
|
||||||
|
|
||||||
|
// Return low memory value...
|
||||||
|
int DCCTimer::getMinimumFreeMemory() {
|
||||||
|
noInterrupts(); // Disable interrupts to get volatile value
|
||||||
|
int retval = minimum_free_memory;
|
||||||
|
interrupts();
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
int DCCTimer::freeMemory() {
|
||||||
|
return ESP.getFreeHeap();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
#ifdef ARDUINO_ARCH_ESP32
|
||||||
|
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
INTERRUPT_CALLBACK interruptHandler=0;
|
||||||
|
|
||||||
|
// https://www.visualmicro.com/page/Timer-Interrupts-Explained.aspx
|
||||||
|
|
||||||
|
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
|
||||||
|
|
||||||
|
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||||
|
interruptHandler = callback;
|
||||||
|
hw_timer_t *timer = NULL;
|
||||||
|
timer = timerBegin(0, 2, true); // prescaler can be 2 to 65536 so choose 2
|
||||||
|
timerAttachInterrupt(timer, interruptHandler, true);
|
||||||
|
timerAlarmWrite(timer, CLOCK_CYCLES / 6, true); // divide by prescaler*3 (Clockbase is 80Mhz and not F_CPU 240Mhz)
|
||||||
|
timerAlarmEnable(timer);
|
||||||
|
}
|
||||||
|
|
||||||
|
// We do not support to use PWM to make the Waveform on ESP
|
||||||
|
bool IRAM_ATTR DCCTimer::isPWMPin(byte pin) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
void IRAM_ATTR DCCTimer::setPWM(byte pin, bool high) {
|
||||||
|
}
|
||||||
|
|
||||||
|
// Fake this as it should not be used
|
||||||
|
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||||
|
mac[0] = 0xFE;
|
||||||
|
mac[1] = 0xBE;
|
||||||
|
mac[2] = 0xEF;
|
||||||
|
mac[3] = 0xC0;
|
||||||
|
mac[4] = 0xFF;
|
||||||
|
mac[5] = 0xEE;
|
||||||
|
}
|
||||||
|
|
||||||
|
volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
|
||||||
|
|
||||||
|
// Return low memory value...
|
||||||
|
int DCCTimer::getMinimumFreeMemory() {
|
||||||
|
noInterrupts(); // Disable interrupts to get volatile value
|
||||||
|
int retval = minimum_free_memory;
|
||||||
|
interrupts();
|
||||||
|
return retval;
|
||||||
|
}
|
||||||
|
|
||||||
|
int DCCTimer::freeMemory() {
|
||||||
|
return ESP.getFreeHeap();
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@@ -47,14 +47,17 @@
|
|||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
// ATTENTION: this file only compiles on a UnoWifiRev3 or NanoEvery
|
||||||
|
// Please refer to DCCTimer.h for general comments about how this class works
|
||||||
|
// This is to avoid repetition and duplication.
|
||||||
|
#ifdef ARDUINO_ARCH_MEGAAVR
|
||||||
|
|
||||||
#include "DCCTimer.h"
|
#include "DCCTimer.h"
|
||||||
const int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle
|
|
||||||
const long CLOCK_CYCLES=(F_CPU / 1000000 * DCC_SIGNAL_TIME) >>1;
|
|
||||||
|
|
||||||
INTERRUPT_CALLBACK interruptHandler=0;
|
INTERRUPT_CALLBACK interruptHandler=0;
|
||||||
|
extern char *__brkval;
|
||||||
|
extern char *__malloc_heap_start;
|
||||||
|
|
||||||
#ifdef ARDUINO_ARCH_MEGAAVR
|
|
||||||
// Arduino unoWifi Rev2 and nanoEvery architectire
|
|
||||||
|
|
||||||
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||||
interruptHandler=callback;
|
interruptHandler=callback;
|
||||||
@@ -87,132 +90,32 @@ INTERRUPT_CALLBACK interruptHandler=0;
|
|||||||
// TODO what are the relevant pins?
|
// TODO what are the relevant pins?
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void DCCTimer::clearPWM() {
|
||||||
|
// Do nothing unless we implent HA
|
||||||
|
}
|
||||||
|
|
||||||
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||||
memcpy(mac,(void *) &SIGROW.SERNUM0,6); // serial number
|
memcpy(mac,(void *) &SIGROW.SERNUM0,6); // serial number
|
||||||
mac[0] &= 0xFE;
|
mac[0] &= 0xFE;
|
||||||
mac[0] |= 0x02;
|
mac[0] |= 0x02;
|
||||||
}
|
}
|
||||||
|
|
||||||
#elif defined(TEENSYDUINO)
|
volatile int DCCTimer::minimum_free_memory=__INT_MAX__;
|
||||||
IntervalTimer myDCCTimer;
|
|
||||||
|
|
||||||
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
// Return low memory value...
|
||||||
interruptHandler=callback;
|
int DCCTimer::getMinimumFreeMemory() {
|
||||||
|
noInterrupts(); // Disable interrupts to get volatile value
|
||||||
myDCCTimer.begin(interruptHandler, DCC_SIGNAL_TIME);
|
int retval = minimum_free_memory;
|
||||||
|
interrupts();
|
||||||
}
|
return retval;
|
||||||
|
|
||||||
bool DCCTimer::isPWMPin(byte pin) {
|
|
||||||
//Teensy: digitalPinHasPWM, todo
|
|
||||||
(void) pin;
|
|
||||||
return false; // TODO what are the relevant pins?
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCCTimer::setPWM(byte pin, bool high) {
|
|
||||||
// TODO what are the relevant pins?
|
|
||||||
(void) pin;
|
|
||||||
(void) high;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
extern char *__brkval;
|
||||||
#if defined(__IMXRT1062__) //Teensy 4.0 and Teensy 4.1
|
extern char *__malloc_heap_start;
|
||||||
uint32_t m1 = HW_OCOTP_MAC1;
|
|
||||||
uint32_t m2 = HW_OCOTP_MAC0;
|
|
||||||
mac[0] = m1 >> 8;
|
|
||||||
mac[1] = m1 >> 0;
|
|
||||||
mac[2] = m2 >> 24;
|
|
||||||
mac[3] = m2 >> 16;
|
|
||||||
mac[4] = m2 >> 8;
|
|
||||||
mac[5] = m2 >> 0;
|
|
||||||
#else
|
|
||||||
read_mac(mac);
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
#if !defined(__IMXRT1062__)
|
int DCCTimer::freeMemory() {
|
||||||
void DCCTimer::read_mac(byte mac[6]) {
|
char top;
|
||||||
read(0xe,mac,0);
|
return __brkval ? &top - __brkval : &top - __malloc_heap_start;
|
||||||
read(0xf,mac,3);
|
|
||||||
}
|
|
||||||
|
|
||||||
// http://forum.pjrc.com/threads/91-teensy-3-MAC-address
|
|
||||||
void DCCTimer::read(uint8_t word, uint8_t *mac, uint8_t offset) {
|
|
||||||
FTFL_FCCOB0 = 0x41; // Selects the READONCE command
|
|
||||||
FTFL_FCCOB1 = word; // read the given word of read once area
|
|
||||||
|
|
||||||
// launch command and wait until complete
|
|
||||||
FTFL_FSTAT = FTFL_FSTAT_CCIF;
|
|
||||||
while(!(FTFL_FSTAT & FTFL_FSTAT_CCIF));
|
|
||||||
|
|
||||||
*(mac+offset) = FTFL_FCCOB5; // collect only the top three bytes,
|
|
||||||
*(mac+offset+1) = FTFL_FCCOB6; // in the right orientation (big endian).
|
|
||||||
*(mac+offset+2) = FTFL_FCCOB7; // Skip FTFL_FCCOB4 as it's always 0.
|
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
#else
|
|
||||||
// Arduino nano, uno, mega etc
|
|
||||||
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
|
|
||||||
#define TIMER1_A_PIN 11
|
|
||||||
#define TIMER1_B_PIN 12
|
|
||||||
#define TIMER1_C_PIN 13
|
|
||||||
#else
|
|
||||||
#define TIMER1_A_PIN 9
|
|
||||||
#define TIMER1_B_PIN 10
|
|
||||||
#endif
|
|
||||||
|
|
||||||
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
|
||||||
interruptHandler=callback;
|
|
||||||
noInterrupts();
|
|
||||||
ADCSRA = (ADCSRA & 0b11111000) | 0b00000100; // speed up analogRead sample time
|
|
||||||
TCCR1A = 0;
|
|
||||||
ICR1 = CLOCK_CYCLES;
|
|
||||||
TCNT1 = 0;
|
|
||||||
TCCR1B = _BV(WGM13) | _BV(CS10); // Mode 8, clock select 1
|
|
||||||
TIMSK1 = _BV(TOIE1); // Enable Software interrupt
|
|
||||||
interrupts();
|
|
||||||
}
|
|
||||||
|
|
||||||
// ISR called by timer interrupt every 58uS
|
|
||||||
ISR(TIMER1_OVF_vect){ interruptHandler(); }
|
|
||||||
|
|
||||||
// Alternative pin manipulation via PWM control.
|
|
||||||
bool DCCTimer::isPWMPin(byte pin) {
|
|
||||||
return pin==TIMER1_A_PIN
|
|
||||||
|| pin==TIMER1_B_PIN
|
|
||||||
#ifdef TIMER1_C_PIN
|
|
||||||
|| pin==TIMER1_C_PIN
|
|
||||||
#endif
|
|
||||||
;
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCCTimer::setPWM(byte pin, bool high) {
|
|
||||||
if (pin==TIMER1_A_PIN) {
|
|
||||||
TCCR1A |= _BV(COM1A1);
|
|
||||||
OCR1A= high?1024:0;
|
|
||||||
}
|
|
||||||
else if (pin==TIMER1_B_PIN) {
|
|
||||||
TCCR1A |= _BV(COM1B1);
|
|
||||||
OCR1B= high?1024:0;
|
|
||||||
}
|
|
||||||
#ifdef TIMER1_C_PIN
|
|
||||||
else if (pin==TIMER1_C_PIN) {
|
|
||||||
TCCR1A |= _BV(COM1C1);
|
|
||||||
OCR1C= high?1024:0;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
#include <avr/boot.h>
|
|
||||||
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
|
||||||
for (byte i=0; i<6; i++) {
|
|
||||||
mac[i]=boot_signature_byte_get(0x0E + i);
|
|
||||||
}
|
|
||||||
mac[0] &= 0xFE;
|
|
||||||
mac[0] |= 0x02;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
#endif
|
126
DCCTimerTEENSY.cpp
Normal file
126
DCCTimerTEENSY.cpp
Normal file
@@ -0,0 +1,126 @@
|
|||||||
|
/*
|
||||||
|
* © 2021 Mike S
|
||||||
|
* © 2021 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 TEENSY
|
||||||
|
// Please refer to DCCTimer.h for general comments about how this class works
|
||||||
|
// This is to avoid repetition and duplication.
|
||||||
|
#ifdef TEENSYDUINO
|
||||||
|
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
|
||||||
|
INTERRUPT_CALLBACK interruptHandler=0;
|
||||||
|
|
||||||
|
IntervalTimer myDCCTimer;
|
||||||
|
|
||||||
|
void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||||
|
interruptHandler=callback;
|
||||||
|
myDCCTimer.begin(interruptHandler, DCC_SIGNAL_TIME);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool DCCTimer::isPWMPin(byte pin) {
|
||||||
|
//Teensy: digitalPinHasPWM, todo
|
||||||
|
(void) pin;
|
||||||
|
return false; // TODO what are the relevant pins?
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCTimer::setPWM(byte pin, bool high) {
|
||||||
|
// TODO what are the relevant pins?
|
||||||
|
(void) pin;
|
||||||
|
(void) high;
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCTimer::clearPWM() {
|
||||||
|
// Do nothing unless we implent HA
|
||||||
|
}
|
||||||
|
|
||||||
|
#if defined(__IMXRT1062__) //Teensy 4.0 and Teensy 4.1
|
||||||
|
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||||
|
uint32_t m1 = HW_OCOTP_MAC1;
|
||||||
|
uint32_t m2 = HW_OCOTP_MAC0;
|
||||||
|
mac[0] = m1 >> 8;
|
||||||
|
mac[1] = m1 >> 0;
|
||||||
|
mac[2] = m2 >> 24;
|
||||||
|
mac[3] = m2 >> 16;
|
||||||
|
mac[4] = m2 >> 8;
|
||||||
|
mac[5] = m2 >> 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
// http://forum.pjrc.com/threads/91-teensy-3-MAC-address
|
||||||
|
void teensyRead(uint8_t word, uint8_t *mac, uint8_t offset) {
|
||||||
|
FTFL_FCCOB0 = 0x41; // Selects the READONCE command
|
||||||
|
FTFL_FCCOB1 = word; // read the given word of read once area
|
||||||
|
|
||||||
|
// launch command and wait until complete
|
||||||
|
FTFL_FSTAT = FTFL_FSTAT_CCIF;
|
||||||
|
while(!(FTFL_FSTAT & FTFL_FSTAT_CCIF));
|
||||||
|
|
||||||
|
*(mac+offset) = FTFL_FCCOB5; // collect only the top three bytes,
|
||||||
|
*(mac+offset+1) = FTFL_FCCOB6; // in the right orientation (big endian).
|
||||||
|
*(mac+offset+2) = FTFL_FCCOB7; // Skip FTFL_FCCOB4 as it's always 0.
|
||||||
|
}
|
||||||
|
|
||||||
|
void DCCTimer::getSimulatedMacAddress(byte mac[6]) {
|
||||||
|
teensyRead(0xe,mac,0);
|
||||||
|
teensyRead(0xf,mac,3);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if !defined(__IMXRT1062__)
|
||||||
|
static inline int freeMemory() {
|
||||||
|
char top;
|
||||||
|
return &top - reinterpret_cast<char*>(sbrk(0));
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
#if defined(ARDUINO_TEENSY40)
|
||||||
|
static const unsigned DTCM_START = 0x20000000UL;
|
||||||
|
static const unsigned OCRAM_START = 0x20200000UL;
|
||||||
|
static const unsigned OCRAM_SIZE = 512;
|
||||||
|
static const unsigned FLASH_SIZE = 1984;
|
||||||
|
#elif defined(ARDUINO_TEENSY41)
|
||||||
|
static const unsigned DTCM_START = 0x20000000UL;
|
||||||
|
static const unsigned OCRAM_START = 0x20200000UL;
|
||||||
|
static const unsigned OCRAM_SIZE = 512;
|
||||||
|
static const unsigned FLASH_SIZE = 7936;
|
||||||
|
#if TEENSYDUINO>151
|
||||||
|
extern "C" uint8_t external_psram_size;
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
static inline int freeMemory() {
|
||||||
|
extern unsigned long _ebss;
|
||||||
|
extern unsigned long _sdata;
|
||||||
|
extern unsigned long _estack;
|
||||||
|
const unsigned DTCM_START = 0x20000000UL;
|
||||||
|
unsigned dtcm = (unsigned)&_estack - DTCM_START;
|
||||||
|
unsigned stackinuse = (unsigned) &_estack - (unsigned) __builtin_frame_address(0);
|
||||||
|
unsigned varsinuse = (unsigned)&_ebss - (unsigned)&_sdata;
|
||||||
|
unsigned freemem = dtcm - (stackinuse + varsinuse);
|
||||||
|
return freemem;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
#endif
|
254
DCCWaveform.cpp
254
DCCWaveform.cpp
@@ -21,43 +21,52 @@
|
|||||||
* You should have received a copy of the GNU General Public License
|
* You should have received a copy of the GNU General Public License
|
||||||
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
* along with CommandStation. If not, see <https://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
#ifndef ARDUINO_ARCH_ESP32
|
||||||
|
// This code is replaced entirely on an ESP32
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
|
||||||
#include "DCCWaveform.h"
|
#include "DCCWaveform.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
#include "DCCTimer.h"
|
#include "DCCTimer.h"
|
||||||
|
#include "DCCACK.h"
|
||||||
#include "DIAG.h"
|
#include "DIAG.h"
|
||||||
#include "freeMemory.h"
|
|
||||||
|
|
||||||
DCCWaveform DCCWaveform::mainTrack(PREAMBLE_BITS_MAIN, true);
|
DCCWaveform DCCWaveform::mainTrack(PREAMBLE_BITS_MAIN, true);
|
||||||
DCCWaveform DCCWaveform::progTrack(PREAMBLE_BITS_PROG, false);
|
DCCWaveform DCCWaveform::progTrack(PREAMBLE_BITS_PROG, false);
|
||||||
|
|
||||||
bool DCCWaveform::progTrackSyncMain=false;
|
|
||||||
bool DCCWaveform::progTrackBoosted=false;
|
|
||||||
int DCCWaveform::progTripValue=0;
|
|
||||||
volatile uint8_t DCCWaveform::numAckGaps=0;
|
|
||||||
volatile uint8_t DCCWaveform::numAckSamples=0;
|
|
||||||
uint8_t DCCWaveform::trailingEdgeCounter=0;
|
|
||||||
|
|
||||||
void DCCWaveform::begin(MotorDriver * mainDriver, MotorDriver * progDriver) {
|
// This bitmask has 9 entries as each byte is trasmitted as a zero + 8 bits.
|
||||||
mainTrack.motorDriver=mainDriver;
|
const byte bitMask[] = {0x00, 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
|
||||||
progTrack.motorDriver=progDriver;
|
|
||||||
progTripValue = progDriver->mA2raw(TRIP_CURRENT_PROG); // need only calculate once hence static
|
const byte idlePacket[] = {0xFF, 0x00, 0xFF};
|
||||||
mainTrack.setPowerMode(POWERMODE::OFF);
|
const byte resetPacket[] = {0x00, 0x00, 0x00};
|
||||||
progTrack.setPowerMode(POWERMODE::OFF);
|
|
||||||
// Fault pin config for odd motor boards (example pololu)
|
|
||||||
MotorDriver::commonFaultPin = ((mainDriver->getFaultPin() == progDriver->getFaultPin())
|
// For each state of the wave nextState=stateTransform[currentState]
|
||||||
&& (mainDriver->getFaultPin() != UNUSED_PIN));
|
const WAVE_STATE stateTransform[]={
|
||||||
// Only use PWM if both pins are PWM capable. Otherwise JOIN does not work
|
/* WAVE_START -> */ WAVE_PENDING,
|
||||||
MotorDriver::usePWM= mainDriver->isPWMCapable() && progDriver->isPWMCapable();
|
/* WAVE_MID_1 -> */ WAVE_START,
|
||||||
DIAG(F("Signal pin config: %S accuracy waveform"),
|
/* WAVE_HIGH_0 -> */ WAVE_MID_0,
|
||||||
MotorDriver::usePWM ? F("high") : F("normal") );
|
/* WAVE_MID_0 -> */ WAVE_LOW_0,
|
||||||
|
/* WAVE_LOW_0 -> */ WAVE_START,
|
||||||
|
/* WAVE_PENDING (should not happen) -> */ WAVE_PENDING};
|
||||||
|
|
||||||
|
// For each state of the wave, signal pin is HIGH or LOW
|
||||||
|
const bool signalTransform[]={
|
||||||
|
/* WAVE_START -> */ HIGH,
|
||||||
|
/* WAVE_MID_1 -> */ LOW,
|
||||||
|
/* WAVE_HIGH_0 -> */ HIGH,
|
||||||
|
/* WAVE_MID_0 -> */ LOW,
|
||||||
|
/* WAVE_LOW_0 -> */ LOW,
|
||||||
|
/* WAVE_PENDING (should not happen) -> */ LOW};
|
||||||
|
|
||||||
|
void DCCWaveform::begin() {
|
||||||
DCCTimer::begin(DCCWaveform::interruptHandler);
|
DCCTimer::begin(DCCWaveform::interruptHandler);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DCCWaveform::loop(bool ackManagerActive) {
|
void DCCWaveform::loop() {
|
||||||
mainTrack.checkPowerOverload(false);
|
// empty placemarker in case ESP32 needs something here
|
||||||
progTrack.checkPowerOverload(ackManagerActive);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#pragma GCC push_options
|
#pragma GCC push_options
|
||||||
@@ -66,11 +75,11 @@ void DCCWaveform::interruptHandler() {
|
|||||||
// call the timer edge sensitive actions for progtrack and maintrack
|
// call the timer edge sensitive actions for progtrack and maintrack
|
||||||
// member functions would be cleaner but have more overhead
|
// member functions would be cleaner but have more overhead
|
||||||
byte sigMain=signalTransform[mainTrack.state];
|
byte sigMain=signalTransform[mainTrack.state];
|
||||||
byte sigProg=progTrackSyncMain? sigMain : signalTransform[progTrack.state];
|
byte sigProg=TrackManager::progTrackSyncMain? sigMain : signalTransform[progTrack.state];
|
||||||
|
|
||||||
// Set the signal state for both tracks
|
// Set the signal state for both tracks
|
||||||
mainTrack.motorDriver->setSignal(sigMain);
|
TrackManager::setDCCSignal(sigMain);
|
||||||
progTrack.motorDriver->setSignal(sigProg);
|
TrackManager::setPROGSignal(sigProg);
|
||||||
|
|
||||||
// Move on in the state engine
|
// Move on in the state engine
|
||||||
mainTrack.state=stateTransform[mainTrack.state];
|
mainTrack.state=stateTransform[mainTrack.state];
|
||||||
@@ -80,7 +89,7 @@ void DCCWaveform::interruptHandler() {
|
|||||||
// WAVE_PENDING means we dont yet know what the next bit is
|
// WAVE_PENDING means we dont yet know what the next bit is
|
||||||
if (mainTrack.state==WAVE_PENDING) mainTrack.interrupt2();
|
if (mainTrack.state==WAVE_PENDING) mainTrack.interrupt2();
|
||||||
if (progTrack.state==WAVE_PENDING) progTrack.interrupt2();
|
if (progTrack.state==WAVE_PENDING) progTrack.interrupt2();
|
||||||
else if (progTrack.ackPending) progTrack.checkAck();
|
else DCCACK::checkAck(progTrack.sentResetsSincePacket);
|
||||||
|
|
||||||
}
|
}
|
||||||
#pragma GCC push_options
|
#pragma GCC push_options
|
||||||
@@ -91,9 +100,6 @@ void DCCWaveform::interruptHandler() {
|
|||||||
// When the current buffer is exhausted, either the pending buffer (if there is one waiting) or an idle buffer.
|
// When the current buffer is exhausted, either the pending buffer (if there is one waiting) or an idle buffer.
|
||||||
|
|
||||||
|
|
||||||
// This bitmask has 9 entries as each byte is trasmitted as a zero + 8 bits.
|
|
||||||
const byte bitMask[] = {0x00, 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01};
|
|
||||||
|
|
||||||
|
|
||||||
DCCWaveform::DCCWaveform( byte preambleBits, bool isMain) {
|
DCCWaveform::DCCWaveform( byte preambleBits, bool isMain) {
|
||||||
isMainTrack = isMain;
|
isMainTrack = isMain;
|
||||||
@@ -105,106 +111,11 @@ DCCWaveform::DCCWaveform( byte preambleBits, bool isMain) {
|
|||||||
requiredPreambles = preambleBits+1;
|
requiredPreambles = preambleBits+1;
|
||||||
bytes_sent = 0;
|
bytes_sent = 0;
|
||||||
bits_sent = 0;
|
bits_sent = 0;
|
||||||
sampleDelay = 0;
|
|
||||||
lastSampleTaken = millis();
|
|
||||||
ackPending=false;
|
|
||||||
}
|
|
||||||
|
|
||||||
POWERMODE DCCWaveform::getPowerMode() {
|
|
||||||
return powerMode;
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCCWaveform::setPowerMode(POWERMODE mode) {
|
|
||||||
powerMode = mode;
|
|
||||||
bool ison = (mode == POWERMODE::ON);
|
|
||||||
motorDriver->setPower( ison);
|
|
||||||
sentResetsSincePacket=0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void DCCWaveform::checkPowerOverload(bool ackManagerActive) {
|
|
||||||
if (millis() - lastSampleTaken < sampleDelay) return;
|
|
||||||
lastSampleTaken = millis();
|
|
||||||
int tripValue= motorDriver->getRawCurrentTripValue();
|
|
||||||
if (!isMainTrack && !ackManagerActive && !progTrackSyncMain && !progTrackBoosted)
|
|
||||||
tripValue=progTripValue;
|
|
||||||
|
|
||||||
// Trackname for diag messages later
|
|
||||||
const FSH*trackname = isMainTrack ? F("MAIN") : F("PROG");
|
|
||||||
switch (powerMode) {
|
|
||||||
case POWERMODE::OFF:
|
|
||||||
sampleDelay = POWER_SAMPLE_OFF_WAIT;
|
|
||||||
break;
|
|
||||||
case POWERMODE::ON:
|
|
||||||
// Check current
|
|
||||||
lastCurrent=motorDriver->getCurrentRaw();
|
|
||||||
if (lastCurrent < 0) {
|
|
||||||
// We have a fault pin condition to take care of
|
|
||||||
lastCurrent = -lastCurrent;
|
|
||||||
setPowerMode(POWERMODE::OVERLOAD); // Turn off, decide later how fast to turn on again
|
|
||||||
if (MotorDriver::commonFaultPin) {
|
|
||||||
if (lastCurrent <= tripValue) {
|
|
||||||
setPowerMode(POWERMODE::ON); // maybe other track
|
|
||||||
}
|
|
||||||
// Write this after the fact as we want to turn on as fast as possible
|
|
||||||
// because we don't know which output actually triggered the fault pin
|
|
||||||
DIAG(F("COMMON FAULT PIN ACTIVE - TOGGLED POWER on %S"), trackname);
|
|
||||||
} else {
|
|
||||||
DIAG(F("%S FAULT PIN ACTIVE - OVERLOAD"), trackname);
|
|
||||||
if (lastCurrent < tripValue) {
|
|
||||||
lastCurrent = tripValue; // exaggerate
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (lastCurrent < tripValue) {
|
|
||||||
sampleDelay = POWER_SAMPLE_ON_WAIT;
|
|
||||||
if(power_good_counter<100)
|
|
||||||
power_good_counter++;
|
|
||||||
else
|
|
||||||
if (power_sample_overload_wait>POWER_SAMPLE_OVERLOAD_WAIT) power_sample_overload_wait=POWER_SAMPLE_OVERLOAD_WAIT;
|
|
||||||
} else {
|
|
||||||
setPowerMode(POWERMODE::OVERLOAD);
|
|
||||||
unsigned int mA=motorDriver->raw2mA(lastCurrent);
|
|
||||||
unsigned int maxmA=motorDriver->raw2mA(tripValue);
|
|
||||||
power_good_counter=0;
|
|
||||||
sampleDelay = power_sample_overload_wait;
|
|
||||||
DIAG(F("%S TRACK POWER OVERLOAD current=%d max=%d offtime=%d"), trackname, mA, maxmA, sampleDelay);
|
|
||||||
if (power_sample_overload_wait >= 10000)
|
|
||||||
power_sample_overload_wait = 10000;
|
|
||||||
else
|
|
||||||
power_sample_overload_wait *= 2;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case POWERMODE::OVERLOAD:
|
|
||||||
// Try setting it back on after the OVERLOAD_WAIT
|
|
||||||
setPowerMode(POWERMODE::ON);
|
|
||||||
sampleDelay = POWER_SAMPLE_ON_WAIT;
|
|
||||||
// Debug code....
|
|
||||||
DIAG(F("%S TRACK POWER RESET delay=%d"), trackname, sampleDelay);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
sampleDelay = 999; // cant get here..meaningless statement to avoid compiler warning.
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// For each state of the wave nextState=stateTransform[currentState]
|
|
||||||
const WAVE_STATE DCCWaveform::stateTransform[]={
|
|
||||||
/* WAVE_START -> */ WAVE_PENDING,
|
|
||||||
/* WAVE_MID_1 -> */ WAVE_START,
|
|
||||||
/* WAVE_HIGH_0 -> */ WAVE_MID_0,
|
|
||||||
/* WAVE_MID_0 -> */ WAVE_LOW_0,
|
|
||||||
/* WAVE_LOW_0 -> */ WAVE_START,
|
|
||||||
/* WAVE_PENDING (should not happen) -> */ WAVE_PENDING};
|
|
||||||
|
|
||||||
// For each state of the wave, signal pin is HIGH or LOW
|
|
||||||
const bool DCCWaveform::signalTransform[]={
|
|
||||||
/* WAVE_START -> */ HIGH,
|
|
||||||
/* WAVE_MID_1 -> */ LOW,
|
|
||||||
/* WAVE_HIGH_0 -> */ HIGH,
|
|
||||||
/* WAVE_MID_0 -> */ LOW,
|
|
||||||
/* WAVE_LOW_0 -> */ LOW,
|
|
||||||
/* WAVE_PENDING (should not happen) -> */ LOW};
|
|
||||||
|
|
||||||
#pragma GCC push_options
|
|
||||||
#pragma GCC optimize ("-O3")
|
#pragma GCC optimize ("-O3")
|
||||||
void DCCWaveform::interrupt2() {
|
void DCCWaveform::interrupt2() {
|
||||||
// calculate the next bit to be sent:
|
// calculate the next bit to be sent:
|
||||||
@@ -216,7 +127,7 @@ void DCCWaveform::interrupt2() {
|
|||||||
remainingPreambles--;
|
remainingPreambles--;
|
||||||
// Update free memory diagnostic as we don't have anything else to do this time.
|
// 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.
|
// Allow for checkAck and its called functions using 22 bytes more.
|
||||||
updateMinimumFreeMemory(22);
|
DCCTimer::updateMinimumFreeMemoryISR(22);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -261,8 +172,6 @@ void DCCWaveform::interrupt2() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#pragma GCC pop_options
|
|
||||||
|
|
||||||
|
|
||||||
// Wait until there is no packet pending, then make this pending
|
// Wait until there is no packet pending, then make this pending
|
||||||
void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repeats) {
|
void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repeats) {
|
||||||
@@ -281,89 +190,4 @@ void DCCWaveform::schedulePacket(const byte buffer[], byte byteCount, byte repea
|
|||||||
packetPending = true;
|
packetPending = true;
|
||||||
sentResetsSincePacket=0;
|
sentResetsSincePacket=0;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
// Operations applicable to PROG track ONLY.
|
|
||||||
// (yes I know I could have subclassed the main track but...)
|
|
||||||
|
|
||||||
void DCCWaveform::setAckBaseline() {
|
|
||||||
if (isMainTrack) return;
|
|
||||||
int baseline=motorDriver->getCurrentRaw();
|
|
||||||
ackThreshold= baseline + motorDriver->mA2raw(ackLimitmA);
|
|
||||||
if (Diag::ACK) DIAG(F("ACK baseline=%d/%dmA Threshold=%d/%dmA Duration between %uus and %uus"),
|
|
||||||
baseline,motorDriver->raw2mA(baseline),
|
|
||||||
ackThreshold,motorDriver->raw2mA(ackThreshold),
|
|
||||||
minAckPulseDuration, maxAckPulseDuration);
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCCWaveform::setAckPending() {
|
|
||||||
if (isMainTrack) return;
|
|
||||||
ackMaxCurrent=0;
|
|
||||||
ackPulseStart=0;
|
|
||||||
ackPulseDuration=0;
|
|
||||||
ackDetected=false;
|
|
||||||
ackCheckStart=millis();
|
|
||||||
numAckSamples=0;
|
|
||||||
numAckGaps=0;
|
|
||||||
ackPending=true; // interrupt routines will now take note
|
|
||||||
}
|
|
||||||
|
|
||||||
byte DCCWaveform::getAck() {
|
|
||||||
if (ackPending) return (2); // still waiting
|
|
||||||
if (Diag::ACK) DIAG(F("%S after %dmS max=%d/%dmA pulse=%uuS samples=%d gaps=%d"),ackDetected?F("ACK"):F("NO-ACK"), ackCheckDuration,
|
|
||||||
ackMaxCurrent,motorDriver->raw2mA(ackMaxCurrent), ackPulseDuration, numAckSamples, numAckGaps);
|
|
||||||
if (ackDetected) return (1); // Yes we had an ack
|
|
||||||
return(0); // pending set off but not detected means no ACK.
|
|
||||||
}
|
|
||||||
|
|
||||||
#pragma GCC push_options
|
|
||||||
#pragma GCC optimize ("-O3")
|
|
||||||
void DCCWaveform::checkAck() {
|
|
||||||
// This function operates in interrupt() time so must be fast and can't DIAG
|
|
||||||
if (sentResetsSincePacket > 6) { //ACK timeout
|
|
||||||
ackCheckDuration=millis()-ackCheckStart;
|
|
||||||
ackPending = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
int current=motorDriver->getCurrentRaw();
|
|
||||||
numAckSamples++;
|
|
||||||
if (current > ackMaxCurrent) ackMaxCurrent=current;
|
|
||||||
// An ACK is a pulse lasting between minAckPulseDuration and maxAckPulseDuration uSecs (refer @haba)
|
|
||||||
|
|
||||||
if (current>ackThreshold) {
|
|
||||||
if (trailingEdgeCounter > 0) {
|
|
||||||
numAckGaps++;
|
|
||||||
trailingEdgeCounter = 0;
|
|
||||||
}
|
|
||||||
if (ackPulseStart==0) ackPulseStart=micros(); // leading edge of pulse detected
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// not in pulse
|
|
||||||
if (ackPulseStart==0) return; // keep waiting for leading edge
|
|
||||||
|
|
||||||
// if we reach to this point, we have
|
|
||||||
// detected trailing edge of pulse
|
|
||||||
if (trailingEdgeCounter == 0) {
|
|
||||||
ackPulseDuration=micros()-ackPulseStart;
|
|
||||||
}
|
|
||||||
|
|
||||||
// but we do not trust it yet and return (which will force another
|
|
||||||
// measurement) and first the third time around with low current
|
|
||||||
// the ack detection will be finalized.
|
|
||||||
if (trailingEdgeCounter < 2) {
|
|
||||||
trailingEdgeCounter++;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
trailingEdgeCounter = 0;
|
|
||||||
|
|
||||||
if (ackPulseDuration>=minAckPulseDuration && ackPulseDuration<=maxAckPulseDuration) {
|
|
||||||
ackCheckDuration=millis()-ackCheckStart;
|
|
||||||
ackDetected=true;
|
|
||||||
ackPending=false;
|
|
||||||
transmitRepeats=0; // shortcut remaining repeat packets
|
|
||||||
return; // we have a genuine ACK result
|
|
||||||
}
|
|
||||||
ackPulseStart=0; // We have detected a too-short or too-long pulse so ignore and wait for next leading edge
|
|
||||||
}
|
|
||||||
#pragma GCC pop_options
|
|
111
DCCWaveform.h
111
DCCWaveform.h
@@ -26,106 +26,39 @@
|
|||||||
|
|
||||||
#include "MotorDriver.h"
|
#include "MotorDriver.h"
|
||||||
|
|
||||||
// Wait times for power management. Unit: milliseconds
|
|
||||||
const int POWER_SAMPLE_ON_WAIT = 100;
|
|
||||||
const int POWER_SAMPLE_OFF_WAIT = 1000;
|
|
||||||
const int POWER_SAMPLE_OVERLOAD_WAIT = 20;
|
|
||||||
|
|
||||||
// Number of preamble bits.
|
// Number of preamble bits.
|
||||||
const int PREAMBLE_BITS_MAIN = 16;
|
const int PREAMBLE_BITS_MAIN = 16;
|
||||||
const int PREAMBLE_BITS_PROG = 22;
|
const int PREAMBLE_BITS_PROG = 22;
|
||||||
const byte MAX_PACKET_SIZE = 5; // NMRA standard extended packets, payload size WITHOUT checksum.
|
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
|
// The WAVE_STATE enum is deliberately numbered because a change of order would be catastrophic
|
||||||
// to the transform array.
|
// to the transform array.
|
||||||
enum WAVE_STATE : byte {WAVE_START=0,WAVE_MID_1=1,WAVE_HIGH_0=2,WAVE_MID_0=3,WAVE_LOW_0=4,WAVE_PENDING=5};
|
enum WAVE_STATE : byte {WAVE_START=0,WAVE_MID_1=1,WAVE_HIGH_0=2,WAVE_MID_0=3,WAVE_LOW_0=4,WAVE_PENDING=5};
|
||||||
|
|
||||||
|
|
||||||
// NOTE: static functions are used for the overall controller, then
|
// NOTE: static functions are used for the overall controller, then
|
||||||
// one instance is created for each track.
|
// one instance is created for each track.
|
||||||
|
|
||||||
|
|
||||||
enum class POWERMODE : byte { OFF, ON, OVERLOAD };
|
|
||||||
|
|
||||||
const byte idlePacket[] = {0xFF, 0x00, 0xFF};
|
|
||||||
const byte resetPacket[] = {0x00, 0x00, 0x00};
|
|
||||||
|
|
||||||
class DCCWaveform {
|
class DCCWaveform {
|
||||||
public:
|
public:
|
||||||
DCCWaveform( byte preambleBits, bool isMain);
|
DCCWaveform( byte preambleBits, bool isMain);
|
||||||
static void begin(MotorDriver * mainDriver, MotorDriver * progDriver);
|
static void begin();
|
||||||
static void loop(bool ackManagerActive);
|
static void loop();
|
||||||
static DCCWaveform mainTrack;
|
static DCCWaveform mainTrack;
|
||||||
static DCCWaveform progTrack;
|
static DCCWaveform progTrack;
|
||||||
|
inline void clearRepeats() { transmitRepeats=0; }
|
||||||
void beginTrack();
|
|
||||||
void setPowerMode(POWERMODE);
|
|
||||||
POWERMODE getPowerMode();
|
|
||||||
void checkPowerOverload(bool ackManagerActive);
|
|
||||||
inline int get1024Current() {
|
|
||||||
if (powerMode == POWERMODE::ON)
|
|
||||||
return (int)(lastCurrent*(long int)1024/motorDriver->getRawCurrentTripValue());
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
inline int getCurrentmA() {
|
|
||||||
if (powerMode == POWERMODE::ON)
|
|
||||||
return motorDriver->raw2mA(lastCurrent);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
inline int getMaxmA() {
|
|
||||||
if (maxmA == 0) { //only calculate this for first request, it doesn't change
|
|
||||||
maxmA = motorDriver->raw2mA(motorDriver->getRawCurrentTripValue()); //TODO: replace with actual max value or calc
|
|
||||||
}
|
|
||||||
return maxmA;
|
|
||||||
}
|
|
||||||
inline int getTripmA() {
|
|
||||||
if (tripmA == 0) { //only calculate this for first request, it doesn't change
|
|
||||||
tripmA = motorDriver->raw2mA(motorDriver->getRawCurrentTripValue());
|
|
||||||
}
|
|
||||||
return tripmA;
|
|
||||||
}
|
|
||||||
void schedulePacket(const byte buffer[], byte byteCount, byte repeats);
|
void schedulePacket(const byte buffer[], byte byteCount, byte repeats);
|
||||||
volatile bool packetPending;
|
volatile bool packetPending;
|
||||||
volatile byte sentResetsSincePacket;
|
volatile byte sentResetsSincePacket;
|
||||||
volatile bool autoPowerOff=false;
|
|
||||||
void setAckBaseline(); //prog track only
|
|
||||||
void setAckPending(); //prog track only
|
|
||||||
byte getAck(); //prog track only 0=NACK, 1=ACK 2=keep waiting
|
|
||||||
static bool progTrackSyncMain; // true when prog track is a siding switched to main
|
|
||||||
static bool progTrackBoosted; // true when prog track is not current limited
|
|
||||||
inline void doAutoPowerOff() {
|
|
||||||
if (autoPowerOff) {
|
|
||||||
setPowerMode(POWERMODE::OFF);
|
|
||||||
autoPowerOff=false;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
inline bool canMeasureCurrent() {
|
|
||||||
return motorDriver->canMeasureCurrent();
|
|
||||||
};
|
|
||||||
inline void setAckLimit(int mA) {
|
|
||||||
ackLimitmA = mA;
|
|
||||||
}
|
|
||||||
inline void setMinAckPulseDuration(unsigned int i) {
|
|
||||||
minAckPulseDuration = i;
|
|
||||||
}
|
|
||||||
inline void setMaxAckPulseDuration(unsigned int i) {
|
|
||||||
maxAckPulseDuration = i;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
// For each state of the wave nextState=stateTransform[currentState]
|
private:
|
||||||
static const WAVE_STATE stateTransform[6];
|
|
||||||
|
|
||||||
// For each state of the wave, signal pin is HIGH or LOW
|
|
||||||
static const bool signalTransform[6];
|
|
||||||
|
|
||||||
static void interruptHandler();
|
static void interruptHandler();
|
||||||
void interrupt2();
|
void interrupt2();
|
||||||
void checkAck();
|
|
||||||
|
|
||||||
bool isMainTrack;
|
bool isMainTrack;
|
||||||
MotorDriver* motorDriver;
|
|
||||||
// Transmission controller
|
// Transmission controller
|
||||||
byte transmitPacket[MAX_PACKET_SIZE+1]; // +1 for checksum
|
byte transmitPacket[MAX_PACKET_SIZE+1]; // +1 for checksum
|
||||||
byte transmitLength;
|
byte transmitLength;
|
||||||
@@ -138,38 +71,6 @@ class DCCWaveform {
|
|||||||
byte pendingPacket[MAX_PACKET_SIZE+1]; // +1 for checksum
|
byte pendingPacket[MAX_PACKET_SIZE+1]; // +1 for checksum
|
||||||
byte pendingLength;
|
byte pendingLength;
|
||||||
byte pendingRepeats;
|
byte pendingRepeats;
|
||||||
int lastCurrent;
|
|
||||||
static int progTripValue;
|
|
||||||
int maxmA;
|
|
||||||
int tripmA;
|
|
||||||
|
|
||||||
// current sampling
|
|
||||||
POWERMODE powerMode;
|
|
||||||
unsigned long lastSampleTaken;
|
|
||||||
unsigned int sampleDelay;
|
|
||||||
// Trip current for programming track, 250mA. Change only if you really
|
|
||||||
// need to be non-NMRA-compliant because of decoders that are not either.
|
|
||||||
static const int TRIP_CURRENT_PROG=250;
|
|
||||||
unsigned long power_sample_overload_wait = POWER_SAMPLE_OVERLOAD_WAIT;
|
|
||||||
unsigned int power_good_counter = 0;
|
|
||||||
|
|
||||||
// ACK management (Prog track only)
|
|
||||||
volatile bool ackPending;
|
|
||||||
volatile bool ackDetected;
|
|
||||||
int ackThreshold;
|
|
||||||
int ackLimitmA = 50;
|
|
||||||
int ackMaxCurrent;
|
|
||||||
unsigned long ackCheckStart; // millis
|
|
||||||
unsigned int ackCheckDuration; // millis
|
|
||||||
|
|
||||||
unsigned int ackPulseDuration; // micros
|
|
||||||
unsigned long ackPulseStart; // micros
|
|
||||||
|
|
||||||
unsigned int minAckPulseDuration = 2000; // micros
|
|
||||||
unsigned int maxAckPulseDuration = 20000; // micros
|
|
||||||
|
|
||||||
volatile static uint8_t numAckGaps;
|
|
||||||
volatile static uint8_t numAckSamples;
|
|
||||||
static uint8_t trailingEdgeCounter;
|
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
40
EXRAIL2.cpp
40
EXRAIL2.cpp
@@ -49,7 +49,7 @@
|
|||||||
#include "DCCEXParser.h"
|
#include "DCCEXParser.h"
|
||||||
#include "Turnouts.h"
|
#include "Turnouts.h"
|
||||||
#include "CommandDistributor.h"
|
#include "CommandDistributor.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
|
|
||||||
// Command parsing keywords
|
// Command parsing keywords
|
||||||
const int16_t HASH_KEYWORD_EXRAIL=15435;
|
const int16_t HASH_KEYWORD_EXRAIL=15435;
|
||||||
@@ -487,10 +487,14 @@ void RMFT2::createNewTask(int route, uint16_t cab) {
|
|||||||
void RMFT2::driveLoco(byte speed) {
|
void RMFT2::driveLoco(byte speed) {
|
||||||
if (loco<=0) return; // Prevent broadcast!
|
if (loco<=0) return; // Prevent broadcast!
|
||||||
if (diag) DIAG(F("EXRAIL drive %d %d %d"),loco,speed,forward^invert);
|
if (diag) DIAG(F("EXRAIL drive %d %d %d"),loco,speed,forward^invert);
|
||||||
if (DCCWaveform::mainTrack.getPowerMode()==POWERMODE::OFF) {
|
/* TODO.....
|
||||||
DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON);
|
power on appropriate track if DC or main if dcc
|
||||||
|
if (TrackManager::getMainPowerMode()==POWERMODE::OFF) {
|
||||||
|
TrackManager::setMainPower(POWERMODE::ON);
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
}
|
}
|
||||||
|
**********/
|
||||||
|
|
||||||
DCC::setThrottle(loco,speed, forward^invert);
|
DCC::setThrottle(loco,speed, forward^invert);
|
||||||
speedo=speed;
|
speedo=speed;
|
||||||
}
|
}
|
||||||
@@ -686,12 +690,21 @@ void RMFT2::loop2() {
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case OPCODE_POWEROFF:
|
case OPCODE_POWEROFF:
|
||||||
DCCWaveform::mainTrack.setPowerMode(POWERMODE::OFF);
|
TrackManager::setPower(POWERMODE::OFF);
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::OFF);
|
TrackManager::setJoin(false);
|
||||||
DCC::setProgTrackSyncMain(false);
|
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case OPCODE_SET_TRACK:
|
||||||
|
// operand is trackmode<<8 | track id
|
||||||
|
// If DC/DCX use my loco for DC address
|
||||||
|
{
|
||||||
|
TRACK_MODE mode = (TRACK_MODE)(operand>>8);
|
||||||
|
int16_t cab=(mode==TRACK_MODE_DC || mode==TRACK_MODE_DCX) ? loco : 0;
|
||||||
|
TrackManager::setTrackMode(operand & 0x0F, mode, cab);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
case OPCODE_RESUME:
|
case OPCODE_RESUME:
|
||||||
pausingTask=NULL;
|
pausingTask=NULL;
|
||||||
driveLoco(speedo);
|
driveLoco(speedo);
|
||||||
@@ -843,20 +856,19 @@ void RMFT2::loop2() {
|
|||||||
return;
|
return;
|
||||||
|
|
||||||
case OPCODE_JOIN:
|
case OPCODE_JOIN:
|
||||||
DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON);
|
TrackManager::setPower(POWERMODE::ON);
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::ON);
|
TrackManager::setJoin(true);
|
||||||
DCC::setProgTrackSyncMain(true);
|
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case OPCODE_POWERON:
|
case OPCODE_POWERON:
|
||||||
DCCWaveform::mainTrack.setPowerMode(POWERMODE::ON);
|
TrackManager::setMainPower(POWERMODE::ON);
|
||||||
DCC::setProgTrackSyncMain(false);
|
TrackManager::setJoin(false);
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case OPCODE_UNJOIN:
|
case OPCODE_UNJOIN:
|
||||||
DCC::setProgTrackSyncMain(false);
|
TrackManager::setJoin(false);
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@@ -52,6 +52,7 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
|
|||||||
OPCODE_ROSTER,OPCODE_KILLALL,
|
OPCODE_ROSTER,OPCODE_KILLALL,
|
||||||
OPCODE_ROUTE,OPCODE_AUTOMATION,OPCODE_SEQUENCE,
|
OPCODE_ROUTE,OPCODE_AUTOMATION,OPCODE_SEQUENCE,
|
||||||
OPCODE_ENDTASK,OPCODE_ENDEXRAIL,
|
OPCODE_ENDTASK,OPCODE_ENDEXRAIL,
|
||||||
|
OPCODE_SET_TRACK,
|
||||||
|
|
||||||
// OPcodes below this point are skip-nesting IF operations
|
// OPcodes below this point are skip-nesting IF operations
|
||||||
// placed here so that they may be skipped as a group
|
// placed here so that they may be skipped as a group
|
||||||
|
@@ -109,6 +109,7 @@
|
|||||||
#undef SERVO_TURNOUT
|
#undef SERVO_TURNOUT
|
||||||
#undef SERVO_SIGNAL
|
#undef SERVO_SIGNAL
|
||||||
#undef SET
|
#undef SET
|
||||||
|
#undef SET_TRACK
|
||||||
#undef SETLOCO
|
#undef SETLOCO
|
||||||
#undef SIGNAL
|
#undef SIGNAL
|
||||||
#undef SIGNALH
|
#undef SIGNALH
|
||||||
@@ -211,6 +212,7 @@
|
|||||||
#define SERVO_SIGNAL(vpin,redpos,amberpos,greenpos)
|
#define SERVO_SIGNAL(vpin,redpos,amberpos,greenpos)
|
||||||
#define SERVO_TURNOUT(id,pin,activeAngle,inactiveAngle,profile,description...)
|
#define SERVO_TURNOUT(id,pin,activeAngle,inactiveAngle,profile,description...)
|
||||||
#define SET(pin)
|
#define SET(pin)
|
||||||
|
#define SET_TRACK(track,mode)
|
||||||
#define SETLOCO(loco)
|
#define SETLOCO(loco)
|
||||||
#define SIGNAL(redpin,amberpin,greenpin)
|
#define SIGNAL(redpin,amberpin,greenpin)
|
||||||
#define SIGNALH(redpin,amberpin,greenpin)
|
#define SIGNALH(redpin,amberpin,greenpin)
|
||||||
|
@@ -262,7 +262,7 @@ const FLASH int16_t RMFT2::SignalDefinitions[] = {
|
|||||||
#define PIN_TURNOUT(id,pin,description...) OPCODE_PINTURNOUT,V(id),OPCODE_PAD,V(pin),
|
#define PIN_TURNOUT(id,pin,description...) OPCODE_PINTURNOUT,V(id),OPCODE_PAD,V(pin),
|
||||||
#define POM(cv,value) OPCODE_POM,V(cv),OPCODE_PAD,V(value),
|
#define POM(cv,value) OPCODE_POM,V(cv),OPCODE_PAD,V(value),
|
||||||
#define POWEROFF OPCODE_POWEROFF,0,0,
|
#define POWEROFF OPCODE_POWEROFF,0,0,
|
||||||
#define POWERON OPCODE_POWERON,0,0,
|
#define POWERON OPCODE_POWERON,0,0,
|
||||||
#define PRINT(msg) OPCODE_PRINT,V(__COUNTER__ - StringMacroTracker2),
|
#define PRINT(msg) OPCODE_PRINT,V(__COUNTER__ - StringMacroTracker2),
|
||||||
#define PARSE(msg) PRINT(msg)
|
#define PARSE(msg) PRINT(msg)
|
||||||
#define READ_LOCO OPCODE_READ_LOCO1,0,0,OPCODE_READ_LOCO2,0,0,
|
#define READ_LOCO OPCODE_READ_LOCO1,0,0,OPCODE_READ_LOCO2,0,0,
|
||||||
@@ -285,6 +285,7 @@ const FLASH int16_t RMFT2::SignalDefinitions[] = {
|
|||||||
#define SERVO_SIGNAL(vpin,redpos,amberpos,greenpos)
|
#define SERVO_SIGNAL(vpin,redpos,amberpos,greenpos)
|
||||||
#define SERVO_TURNOUT(id,pin,activeAngle,inactiveAngle,profile,description...) OPCODE_SERVOTURNOUT,V(id),OPCODE_PAD,V(pin),OPCODE_PAD,V(activeAngle),OPCODE_PAD,V(inactiveAngle),OPCODE_PAD,V(PCA9685::ProfileType::profile),
|
#define SERVO_TURNOUT(id,pin,activeAngle,inactiveAngle,profile,description...) OPCODE_SERVOTURNOUT,V(id),OPCODE_PAD,V(pin),OPCODE_PAD,V(activeAngle),OPCODE_PAD,V(inactiveAngle),OPCODE_PAD,V(PCA9685::ProfileType::profile),
|
||||||
#define SET(pin) OPCODE_SET,V(pin),
|
#define SET(pin) OPCODE_SET,V(pin),
|
||||||
|
#define SET_TRACK(track,mode) OPCODE_SET_TRACK,V(TRACK_MODE_##mode <<8 | TRACK_NUMBER_##track),
|
||||||
#define SETLOCO(loco) OPCODE_SETLOCO,V(loco),
|
#define SETLOCO(loco) OPCODE_SETLOCO,V(loco),
|
||||||
#define SIGNAL(redpin,amberpin,greenpin)
|
#define SIGNAL(redpin,amberpin,greenpin)
|
||||||
#define SIGNALH(redpin,amberpin,greenpin)
|
#define SIGNALH(redpin,amberpin,greenpin)
|
||||||
|
@@ -31,7 +31,7 @@
|
|||||||
#include "defines.h"
|
#include "defines.h"
|
||||||
#include "DCCEXParser.h"
|
#include "DCCEXParser.h"
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include <avr/pgmspace.h>
|
//#include <avr/pgmspace.h>
|
||||||
#if defined (ARDUINO_TEENSY41)
|
#if defined (ARDUINO_TEENSY41)
|
||||||
#include <NativeEthernet.h> //TEENSY Ethernet Treiber
|
#include <NativeEthernet.h> //TEENSY Ethernet Treiber
|
||||||
#include <NativeEthernetUdp.h>
|
#include <NativeEthernetUdp.h>
|
||||||
|
@@ -1 +1 @@
|
|||||||
#define GITHUB_SHA "a26d988"
|
#define GITHUB_SHA "TM-PORTX-20220607-1"
|
||||||
|
@@ -69,6 +69,10 @@ protected:
|
|||||||
|
|
||||||
I2CRB requestBlock;
|
I2CRB requestBlock;
|
||||||
FSH *_deviceName;
|
FSH *_deviceName;
|
||||||
|
#if defined(ARDUINO_ARCH_ESP32)
|
||||||
|
// workaround: Has somehow no min function for all types
|
||||||
|
static inline T min(T a, int b) { return a < b ? a : b; };
|
||||||
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
// Because class GPIOBase is a template, the implementation (below) must be contained within the same
|
// Because class GPIOBase is a template, the implementation (below) must be contained within the same
|
||||||
@@ -246,4 +250,4 @@ int GPIOBase<T>::_read(VPIN vpin) {
|
|||||||
return (_portInputState & mask) ? 0 : 1; // Invert state (5v=0, 0v=1)
|
return (_portInputState & mask) ? 0 : 1; // Invert state (5v=0, 0v=1)
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
216
MotorDriver.cpp
216
MotorDriver.cpp
@@ -25,24 +25,39 @@
|
|||||||
#include "DCCTimer.h"
|
#include "DCCTimer.h"
|
||||||
#include "DIAG.h"
|
#include "DIAG.h"
|
||||||
|
|
||||||
#define setHIGH(fastpin) *fastpin.inout |= fastpin.maskHIGH
|
|
||||||
#define setLOW(fastpin) *fastpin.inout &= fastpin.maskLOW
|
|
||||||
#define isHIGH(fastpin) (*fastpin.inout & fastpin.maskHIGH)
|
|
||||||
#define isLOW(fastpin) (!isHIGH(fastpin))
|
|
||||||
|
|
||||||
bool MotorDriver::usePWM=false;
|
bool MotorDriver::usePWM=false;
|
||||||
bool MotorDriver::commonFaultPin=false;
|
bool MotorDriver::commonFaultPin=false;
|
||||||
|
|
||||||
MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin,
|
volatile byte fakePORTA;
|
||||||
|
volatile byte fakePORTB;
|
||||||
|
volatile byte fakePORTC;
|
||||||
|
|
||||||
|
MotorDriver::MotorDriver(VPIN power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin,
|
||||||
byte current_pin, float sense_factor, unsigned int trip_milliamps, byte fault_pin) {
|
byte current_pin, float sense_factor, unsigned int trip_milliamps, byte fault_pin) {
|
||||||
powerPin=power_pin;
|
powerPin=power_pin;
|
||||||
getFastPin(F("POWER"),powerPin,fastPowerPin);
|
IODevice::write(powerPin,LOW);// set to OUTPUT and off
|
||||||
pinMode(powerPin, OUTPUT);
|
|
||||||
|
|
||||||
signalPin=signal_pin;
|
signalPin=signal_pin;
|
||||||
getFastPin(F("SIG"),signalPin,fastSignalPin);
|
getFastPin(F("SIG"),signalPin,fastSignalPin);
|
||||||
pinMode(signalPin, OUTPUT);
|
pinMode(signalPin, OUTPUT);
|
||||||
|
|
||||||
|
fastSignalPin.shadowinout = NULL;
|
||||||
|
if (HAVE_PORTA(fastSignalPin.inout == &PORTA)) {
|
||||||
|
DIAG(F("Found PORTA pin %d"),signalPin);
|
||||||
|
fastSignalPin.shadowinout = fastSignalPin.inout;
|
||||||
|
fastSignalPin.inout = &fakePORTA;
|
||||||
|
}
|
||||||
|
if (HAVE_PORTB(fastSignalPin.inout == &PORTB)) {
|
||||||
|
DIAG(F("Found PORTB pin %d"),signalPin);
|
||||||
|
fastSignalPin.shadowinout = fastSignalPin.inout;
|
||||||
|
fastSignalPin.inout = &fakePORTB;
|
||||||
|
}
|
||||||
|
if (HAVE_PORTC(fastSignalPin.inout == &PORTC)) {
|
||||||
|
DIAG(F("Found PORTC pin %d"),signalPin);
|
||||||
|
fastSignalPin.shadowinout = fastSignalPin.inout;
|
||||||
|
fastSignalPin.inout = &fakePORTC;
|
||||||
|
}
|
||||||
|
|
||||||
signalPin2=signal_pin2;
|
signalPin2=signal_pin2;
|
||||||
if (signalPin2!=UNUSED_PIN) {
|
if (signalPin2!=UNUSED_PIN) {
|
||||||
dualSignal=true;
|
dualSignal=true;
|
||||||
@@ -56,8 +71,9 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8
|
|||||||
invertBrake=brake_pin < 0;
|
invertBrake=brake_pin < 0;
|
||||||
brakePin=invertBrake ? 0-brake_pin : brake_pin;
|
brakePin=invertBrake ? 0-brake_pin : brake_pin;
|
||||||
getFastPin(F("BRAKE"),brakePin,fastBrakePin);
|
getFastPin(F("BRAKE"),brakePin,fastBrakePin);
|
||||||
|
// if brake is used for railcom cutout we need to do PORTX register trick here as well
|
||||||
pinMode(brakePin, OUTPUT);
|
pinMode(brakePin, OUTPUT);
|
||||||
setBrake(false);
|
setBrake(true); // start with brake on in case we hace DC stuff going on
|
||||||
}
|
}
|
||||||
else brakePin=UNUSED_PIN;
|
else brakePin=UNUSED_PIN;
|
||||||
|
|
||||||
@@ -82,6 +98,12 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8
|
|||||||
else
|
else
|
||||||
DIAG(F("MotorDriver currentPin=A%d, senseOffset=%d, rawCurrentTripValue(relative to offset)=%d"),
|
DIAG(F("MotorDriver currentPin=A%d, senseOffset=%d, rawCurrentTripValue(relative to offset)=%d"),
|
||||||
currentPin-A0, senseOffset,rawCurrentTripValue);
|
currentPin-A0, senseOffset,rawCurrentTripValue);
|
||||||
|
|
||||||
|
// prepare values for current detection
|
||||||
|
sampleDelay = 0;
|
||||||
|
lastSampleTaken = millis();
|
||||||
|
progTripValue = mA2raw(TRIP_CURRENT_PROG);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MotorDriver::isPWMCapable() {
|
bool MotorDriver::isPWMCapable() {
|
||||||
@@ -89,15 +111,15 @@ bool MotorDriver::isPWMCapable() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void MotorDriver::setPower(bool on) {
|
void MotorDriver::setPower(POWERMODE mode) {
|
||||||
|
bool on=mode==POWERMODE::ON;
|
||||||
if (on) {
|
if (on) {
|
||||||
// toggle brake before turning power on - resets overcurrent error
|
IODevice::write(powerPin,HIGH);
|
||||||
// on the Pololu board if brake is wired to ^D2.
|
if (resetsCounterP != NULL)
|
||||||
setBrake(true);
|
*resetsCounterP = 0;
|
||||||
setBrake(false);
|
|
||||||
setHIGH(fastPowerPin);
|
|
||||||
}
|
}
|
||||||
else setLOW(fastPowerPin);
|
else IODevice::write(powerPin,LOW);
|
||||||
|
powerMode=mode;
|
||||||
}
|
}
|
||||||
|
|
||||||
// setBrake applies brake if on == true. So to get
|
// setBrake applies brake if on == true. So to get
|
||||||
@@ -114,26 +136,6 @@ void MotorDriver::setBrake(bool on) {
|
|||||||
else setLOW(fastBrakePin);
|
else setLOW(fastBrakePin);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MotorDriver::setSignal( bool high) {
|
|
||||||
if (usePWM) {
|
|
||||||
DCCTimer::setPWM(signalPin,high);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (high) {
|
|
||||||
setHIGH(fastSignalPin);
|
|
||||||
if (dualSignal) setLOW(fastSignalPin2);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
setLOW(fastSignalPin);
|
|
||||||
if (dualSignal) setHIGH(fastSignalPin2);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(ARDUINO_TEENSY32) || defined(ARDUINO_TEENSY35)|| defined(ARDUINO_TEENSY36)
|
|
||||||
volatile unsigned int overflow_count=0;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
bool MotorDriver::canMeasureCurrent() {
|
bool MotorDriver::canMeasureCurrent() {
|
||||||
return currentPin!=UNUSED_PIN;
|
return currentPin!=UNUSED_PIN;
|
||||||
}
|
}
|
||||||
@@ -148,28 +150,72 @@ bool MotorDriver::canMeasureCurrent() {
|
|||||||
int MotorDriver::getCurrentRaw() {
|
int MotorDriver::getCurrentRaw() {
|
||||||
if (currentPin==UNUSED_PIN) return 0;
|
if (currentPin==UNUSED_PIN) return 0;
|
||||||
int current;
|
int current;
|
||||||
#if defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41)
|
// This function should NOT be called in an interruot so we
|
||||||
bool irq = disableInterrupts();
|
// dont need to fart about saving and restoring CPU specific
|
||||||
|
// interrupt registers.
|
||||||
|
noInterrupts();
|
||||||
current = analogRead(currentPin)-senseOffset;
|
current = analogRead(currentPin)-senseOffset;
|
||||||
enableInterrupts(irq);
|
interrupts();
|
||||||
#else // Uno, Mega and all the TEENSY3* but not TEENSY4*
|
|
||||||
unsigned char sreg_backup;
|
|
||||||
sreg_backup = SREG; /* save interrupt enable/disable state */
|
|
||||||
cli();
|
|
||||||
current = analogRead(currentPin)-senseOffset;
|
|
||||||
#if defined(ARDUINO_TEENSY32) || defined(ARDUINO_TEENSY35)|| defined(ARDUINO_TEENSY36)
|
|
||||||
overflow_count = 0;
|
|
||||||
#endif
|
|
||||||
if (sreg_backup & 128) sei(); /* restore interrupt state */
|
|
||||||
#endif // outer #
|
|
||||||
if (current<0) current=0-current;
|
if (current<0) current=0-current;
|
||||||
if ((faultPin != UNUSED_PIN) && isLOW(fastFaultPin) && isHIGH(fastPowerPin))
|
if ((faultPin != UNUSED_PIN) && isLOW(fastFaultPin) && powerMode==POWERMODE::ON)
|
||||||
return (current == 0 ? -1 : -current);
|
return (current == 0 ? -1 : -current);
|
||||||
return current;
|
return current;
|
||||||
// IMPORTANT: This function can be called in Interrupt() time within the 56uS timer
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotorDriver::setDCSignal(byte speedcode) {
|
||||||
|
if (brakePin == UNUSED_PIN)
|
||||||
|
return;
|
||||||
|
// spedcoode is a dcc speed & direction
|
||||||
|
byte tSpeed=speedcode & 0x7F; // DCC Speed with 0,1 stop and speed steps 2 to 127
|
||||||
|
byte tDir=speedcode & 0x80;
|
||||||
|
byte brake;
|
||||||
|
if (tSpeed <= 1) brake = 255;
|
||||||
|
else if (tSpeed >= 127) brake = 0;
|
||||||
|
else brake = 2 * (128-tSpeed);
|
||||||
|
DIAG(F("setDCSignal: speedcode=%d BrakePin=%d brake=%d dir=%d"),speedcode, brakePin, brake, tDir);
|
||||||
|
analogWrite(brakePin,brake);
|
||||||
|
// as the port registers can be shadowed to get syncronized DCC signals
|
||||||
|
// we need to take care of that and we have to turn off interrupts during
|
||||||
|
// that time as otherwise setDCCSignal() which is called from interrupt
|
||||||
|
// contect can undo whatever we do here.
|
||||||
|
if (fastSignalPin.shadowinout != NULL) {
|
||||||
|
if (HAVE_PORTA(fastSignalPin.shadowinout == &PORTA)) {
|
||||||
|
DIAG(F("setDCSignal: HAVEPORTA"));
|
||||||
|
noInterrupts();
|
||||||
|
HAVE_PORTA(fakePORTA=PORTA);
|
||||||
|
setSignal(tDir);
|
||||||
|
HAVE_PORTA(PORTA=fakePORTA);
|
||||||
|
interrupts();
|
||||||
|
} else if (HAVE_PORTB(fastSignalPin.shadowinout == &PORTB)) {
|
||||||
|
DIAG(F("setDCSignal: HAVEPORTB"));
|
||||||
|
noInterrupts();
|
||||||
|
HAVE_PORTB(fakePORTB=PORTB);
|
||||||
|
setSignal(tDir);
|
||||||
|
HAVE_PORTB(PORTB=fakePORTB);
|
||||||
|
interrupts();
|
||||||
|
} else if (HAVE_PORTC(fastSignalPin.shadowinout == &PORTC)) {
|
||||||
|
DIAG(F("setDCSignal: HAVEPORTC"));
|
||||||
|
noInterrupts();
|
||||||
|
HAVE_PORTC(fakePORTC=PORTC);
|
||||||
|
setSignal(tDir);
|
||||||
|
HAVE_PORTC(PORTC=fakePORTC);
|
||||||
|
interrupts();
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
setSignal(tDir);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int MotorDriver::getCurrentRawInInterrupt() {
|
||||||
|
|
||||||
|
// IMPORTANT: This function must be called in Interrupt() time within the 56uS timer
|
||||||
// The default analogRead takes ~100uS which is catastrphic
|
// The default analogRead takes ~100uS which is catastrphic
|
||||||
// so DCCTimer has set the sample time to be much faster.
|
// so DCCTimer has set the sample time to be much faster.
|
||||||
}
|
|
||||||
|
if (currentPin==UNUSED_PIN) return 0;
|
||||||
|
return analogRead(currentPin)-senseOffset;
|
||||||
|
}
|
||||||
|
|
||||||
unsigned int MotorDriver::raw2mA( int raw) {
|
unsigned int MotorDriver::raw2mA( int raw) {
|
||||||
return (unsigned int)(raw * senseFactor);
|
return (unsigned int)(raw * senseFactor);
|
||||||
@@ -190,3 +236,65 @@ void MotorDriver::getFastPin(const FSH* type,int pin, bool input, FASTPIN & res
|
|||||||
result.maskLOW = ~result.maskHIGH;
|
result.maskLOW = ~result.maskHIGH;
|
||||||
// DIAG(F(" port=0x%x, inoutpin=0x%x, isinput=%d, mask=0x%x"),port, result.inout,input,result.maskHIGH);
|
// DIAG(F(" port=0x%x, inoutpin=0x%x, isinput=%d, mask=0x%x"),port, result.inout,input,result.maskHIGH);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void MotorDriver::checkPowerOverload(bool useProgLimit, byte trackno) {
|
||||||
|
if (millis() - lastSampleTaken < sampleDelay) return;
|
||||||
|
lastSampleTaken = millis();
|
||||||
|
int tripValue= useProgLimit?progTripValue:getRawCurrentTripValue();
|
||||||
|
|
||||||
|
// Trackname for diag messages later
|
||||||
|
switch (powerMode) {
|
||||||
|
case POWERMODE::OFF:
|
||||||
|
sampleDelay = POWER_SAMPLE_OFF_WAIT;
|
||||||
|
break;
|
||||||
|
case POWERMODE::ON:
|
||||||
|
// Check current
|
||||||
|
lastCurrent=getCurrentRaw();
|
||||||
|
if (lastCurrent < 0) {
|
||||||
|
// We have a fault pin condition to take care of
|
||||||
|
lastCurrent = -lastCurrent;
|
||||||
|
setPower(POWERMODE::OVERLOAD); // Turn off, decide later how fast to turn on again
|
||||||
|
if (commonFaultPin) {
|
||||||
|
if (lastCurrent <= tripValue) {
|
||||||
|
setPower(POWERMODE::ON); // maybe other track
|
||||||
|
}
|
||||||
|
// Write this after the fact as we want to turn on as fast as possible
|
||||||
|
// because we don't know which output actually triggered the fault pin
|
||||||
|
DIAG(F("COMMON FAULT PIN ACTIVE - TOGGLED POWER on %d"), trackno);
|
||||||
|
} else {
|
||||||
|
DIAG(F("TRACK %d FAULT PIN ACTIVE - OVERLOAD"), trackno);
|
||||||
|
if (lastCurrent < tripValue) {
|
||||||
|
lastCurrent = tripValue; // exaggerate
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (lastCurrent < tripValue) {
|
||||||
|
sampleDelay = POWER_SAMPLE_ON_WAIT;
|
||||||
|
if(power_good_counter<100)
|
||||||
|
power_good_counter++;
|
||||||
|
else
|
||||||
|
if (power_sample_overload_wait>POWER_SAMPLE_OVERLOAD_WAIT) power_sample_overload_wait=POWER_SAMPLE_OVERLOAD_WAIT;
|
||||||
|
} else {
|
||||||
|
setPower(POWERMODE::OVERLOAD);
|
||||||
|
unsigned int mA=raw2mA(lastCurrent);
|
||||||
|
unsigned int maxmA=raw2mA(tripValue);
|
||||||
|
power_good_counter=0;
|
||||||
|
sampleDelay = power_sample_overload_wait;
|
||||||
|
DIAG(F("TRACK %d POWER OVERLOAD current=%d max=%d offtime=%d"), trackno, mA, maxmA, sampleDelay);
|
||||||
|
if (power_sample_overload_wait >= 10000)
|
||||||
|
power_sample_overload_wait = 10000;
|
||||||
|
else
|
||||||
|
power_sample_overload_wait *= 2;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case POWERMODE::OVERLOAD:
|
||||||
|
// Try setting it back on after the OVERLOAD_WAIT
|
||||||
|
setPower(POWERMODE::ON);
|
||||||
|
sampleDelay = POWER_SAMPLE_ON_WAIT;
|
||||||
|
// Debug code....
|
||||||
|
DIAG(F("TRACK %d POWER RESET delay=%d"), trackno, sampleDelay);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
sampleDelay = 999; // cant get here..meaningless statement to avoid compiler warning.
|
||||||
|
}
|
||||||
|
}
|
||||||
|
120
MotorDriver.h
120
MotorDriver.h
@@ -2,6 +2,7 @@
|
|||||||
* © 2021 Mike S
|
* © 2021 Mike S
|
||||||
* © 2021 Fred Decker
|
* © 2021 Fred Decker
|
||||||
* © 2020 Chris Harlow
|
* © 2020 Chris Harlow
|
||||||
|
* © 2022 Harald Barth
|
||||||
* All rights reserved.
|
* All rights reserved.
|
||||||
*
|
*
|
||||||
* This file is part of Asbelos DCC API
|
* This file is part of Asbelos DCC API
|
||||||
@@ -22,6 +23,38 @@
|
|||||||
#ifndef MotorDriver_h
|
#ifndef MotorDriver_h
|
||||||
#define MotorDriver_h
|
#define MotorDriver_h
|
||||||
#include "FSH.h"
|
#include "FSH.h"
|
||||||
|
#include "IODevice.h"
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
|
||||||
|
#define setHIGH(fastpin) *fastpin.inout |= fastpin.maskHIGH
|
||||||
|
#define setLOW(fastpin) *fastpin.inout &= fastpin.maskLOW
|
||||||
|
#define isHIGH(fastpin) (*fastpin.inout & fastpin.maskHIGH)
|
||||||
|
#define isLOW(fastpin) (!isHIGH(fastpin))
|
||||||
|
|
||||||
|
#define TOKENPASTE(x, y) x ## y
|
||||||
|
#define TOKENPASTE2(x, y) TOKENPASTE(x, y)
|
||||||
|
|
||||||
|
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
|
||||||
|
#define HAVE_PORTA(X) X
|
||||||
|
#define HAVE_PORTB(X) X
|
||||||
|
#define HAVE_PORTC(X) X
|
||||||
|
#endif
|
||||||
|
#if defined(ARDUINO_AVR_UNO)
|
||||||
|
#define HAVE_PORTB(X) X
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// if macros not defined as pass-through we define
|
||||||
|
// them here as someting that is valid as a
|
||||||
|
// statement and evaluates to false.
|
||||||
|
#ifndef HAVE_PORTA
|
||||||
|
#define HAVE_PORTA(X) byte TOKENPASTE2(Unique_, __LINE__) __attribute__((unused)) =0
|
||||||
|
#endif
|
||||||
|
#ifndef HAVE_PORTB
|
||||||
|
#define HAVE_PORTB(X) byte TOKENPASTE2(Unique_, __LINE__) __attribute__((unused)) =0
|
||||||
|
#endif
|
||||||
|
#ifndef HAVE_PORTC
|
||||||
|
#define HAVE_PORTC(X) byte TOKENPASTE2(Unique_, __LINE__) __attribute__((unused)) =0
|
||||||
|
#endif
|
||||||
|
|
||||||
// Virtualised Motor shield 1-track hardware Interface
|
// Virtualised Motor shield 1-track hardware Interface
|
||||||
|
|
||||||
@@ -29,63 +62,110 @@
|
|||||||
#define UNUSED_PIN 127 // inside int8_t
|
#define UNUSED_PIN 127 // inside int8_t
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(__IMXRT1062__)
|
#if defined(__IMXRT1062__) || defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)
|
||||||
struct FASTPIN {
|
struct FASTPIN {
|
||||||
volatile uint32_t *inout;
|
volatile uint32_t *inout;
|
||||||
uint32_t maskHIGH;
|
uint32_t maskHIGH;
|
||||||
uint32_t maskLOW;
|
uint32_t maskLOW;
|
||||||
|
volatile uint32_t *shadowinout;
|
||||||
};
|
};
|
||||||
#else
|
#else
|
||||||
struct FASTPIN {
|
struct FASTPIN {
|
||||||
volatile uint8_t *inout;
|
volatile uint8_t *inout;
|
||||||
uint8_t maskHIGH;
|
uint8_t maskHIGH;
|
||||||
uint8_t maskLOW;
|
uint8_t maskLOW;
|
||||||
|
volatile uint8_t *shadowinout;
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
enum class POWERMODE : byte { OFF, ON, OVERLOAD };
|
||||||
|
|
||||||
class MotorDriver {
|
class MotorDriver {
|
||||||
public:
|
public:
|
||||||
MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin,
|
|
||||||
|
MotorDriver(VPIN power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin,
|
||||||
byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin);
|
byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin);
|
||||||
virtual void setPower( bool on);
|
virtual void setPower( POWERMODE mode);
|
||||||
virtual void setSignal( bool high);
|
virtual POWERMODE getPower() { return powerMode;}
|
||||||
|
__attribute__((always_inline)) inline void setSignal( bool high) {
|
||||||
|
if (trackPWM) {
|
||||||
|
DCCTimer::setPWM(signalPin,high);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (high) {
|
||||||
|
setHIGH(fastSignalPin);
|
||||||
|
if (dualSignal) setLOW(fastSignalPin2);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
setLOW(fastSignalPin);
|
||||||
|
if (dualSignal) setHIGH(fastSignalPin2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
inline void enableSignal(bool on) {
|
||||||
|
if (on)
|
||||||
|
pinMode(signalPin, OUTPUT);
|
||||||
|
else
|
||||||
|
pinMode(signalPin, INPUT);
|
||||||
|
};
|
||||||
virtual void setBrake( bool on);
|
virtual void setBrake( bool on);
|
||||||
|
virtual void setDCSignal(byte speedByte);
|
||||||
virtual int getCurrentRaw();
|
virtual int getCurrentRaw();
|
||||||
|
virtual int getCurrentRawInInterrupt();
|
||||||
virtual unsigned int raw2mA( int raw);
|
virtual unsigned int raw2mA( int raw);
|
||||||
virtual int mA2raw( unsigned int mA);
|
virtual int mA2raw( unsigned int mA);
|
||||||
|
inline bool brakeCanPWM() {
|
||||||
|
return ((brakePin!=UNUSED_PIN) && (digitalPinToTimer(brakePin)));
|
||||||
|
}
|
||||||
inline int getRawCurrentTripValue() {
|
inline int getRawCurrentTripValue() {
|
||||||
return rawCurrentTripValue;
|
return rawCurrentTripValue;
|
||||||
}
|
}
|
||||||
bool isPWMCapable();
|
bool isPWMCapable();
|
||||||
bool canMeasureCurrent();
|
bool canMeasureCurrent();
|
||||||
static bool usePWM;
|
bool trackPWM;
|
||||||
|
static bool usePWM; // TODO: Remove
|
||||||
static bool commonFaultPin; // This is a stupid motor shield which has only a common fault pin for both outputs
|
static bool commonFaultPin; // This is a stupid motor shield which has only a common fault pin for both outputs
|
||||||
inline byte getFaultPin() {
|
inline byte getFaultPin() {
|
||||||
return faultPin;
|
return faultPin;
|
||||||
}
|
}
|
||||||
|
inline void setResetCounterPointer(volatile byte *bp) { // load resetPacketCounter pointer
|
||||||
|
resetsCounterP = bp;
|
||||||
|
}
|
||||||
|
void checkPowerOverload(bool useProgLimit, byte trackno);
|
||||||
private:
|
private:
|
||||||
|
volatile byte *resetsCounterP = NULL; // points to the resetPacketCounter if this is a prog track
|
||||||
void getFastPin(const FSH* type,int pin, bool input, FASTPIN & result);
|
void getFastPin(const FSH* type,int pin, bool input, FASTPIN & result);
|
||||||
void getFastPin(const FSH* type,int pin, FASTPIN & result) {
|
void getFastPin(const FSH* type,int pin, FASTPIN & result) {
|
||||||
getFastPin(type, pin, 0, result);
|
getFastPin(type, pin, 0, result);
|
||||||
}
|
}
|
||||||
byte powerPin, signalPin, signalPin2, currentPin, faultPin, brakePin;
|
VPIN powerPin;
|
||||||
FASTPIN fastPowerPin,fastSignalPin, fastSignalPin2, fastBrakePin,fastFaultPin;
|
byte signalPin, signalPin2, currentPin, faultPin, brakePin;
|
||||||
|
FASTPIN fastSignalPin, fastSignalPin2, fastBrakePin,fastFaultPin;
|
||||||
bool dualSignal; // true to use signalPin2
|
bool dualSignal; // true to use signalPin2
|
||||||
bool invertBrake; // brake pin passed as negative means pin is inverted
|
bool invertBrake; // brake pin passed as negative means pin is inverted
|
||||||
float senseFactor;
|
float senseFactor;
|
||||||
int senseOffset;
|
int senseOffset;
|
||||||
unsigned int tripMilliamps;
|
unsigned int tripMilliamps;
|
||||||
int rawCurrentTripValue;
|
int rawCurrentTripValue;
|
||||||
#if defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41)
|
// current sampling
|
||||||
static bool disableInterrupts() {
|
POWERMODE powerMode;
|
||||||
uint32_t primask;
|
unsigned long lastSampleTaken;
|
||||||
__asm__ volatile("mrs %0, primask\n" : "=r" (primask)::);
|
unsigned int sampleDelay;
|
||||||
__disable_irq();
|
int progTripValue;
|
||||||
return (primask == 0) ? true : false;
|
int lastCurrent;
|
||||||
}
|
int maxmA;
|
||||||
static void enableInterrupts(bool doit) {
|
int tripmA;
|
||||||
if (doit) __enable_irq();
|
|
||||||
}
|
// Wait times for power management. Unit: milliseconds
|
||||||
#endif
|
static const int POWER_SAMPLE_ON_WAIT = 100;
|
||||||
|
static const int POWER_SAMPLE_OFF_WAIT = 1000;
|
||||||
|
static const int POWER_SAMPLE_OVERLOAD_WAIT = 20;
|
||||||
|
|
||||||
|
// Trip current for programming track, 250mA. Change only if you really
|
||||||
|
// need to be non-NMRA-compliant because of decoders that are not either.
|
||||||
|
static const int TRIP_CURRENT_PROG=250;
|
||||||
|
unsigned long power_sample_overload_wait = POWER_SAMPLE_OVERLOAD_WAIT;
|
||||||
|
unsigned int power_good_counter = 0;
|
||||||
|
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
@@ -47,8 +47,8 @@
|
|||||||
//
|
//
|
||||||
// Arduino standard Motor Shield
|
// Arduino standard Motor Shield
|
||||||
#define STANDARD_MOTOR_SHIELD F("STANDARD_MOTOR_SHIELD"), \
|
#define STANDARD_MOTOR_SHIELD F("STANDARD_MOTOR_SHIELD"), \
|
||||||
new MotorDriver(3, 12, UNUSED_PIN, UNUSED_PIN, A0, 2.99, 2000, UNUSED_PIN), \
|
new MotorDriver(3, 12, UNUSED_PIN, 9, A0, 2.99, 2000, UNUSED_PIN), \
|
||||||
new MotorDriver(11, 13, UNUSED_PIN, UNUSED_PIN, A1, 2.99, 2000, UNUSED_PIN)
|
new MotorDriver(11, 13, UNUSED_PIN, 8, A1, 2.99, 2000, UNUSED_PIN)
|
||||||
|
|
||||||
// Pololu Motor Shield
|
// Pololu Motor Shield
|
||||||
#define POLOLU_MOTOR_SHIELD F("POLOLU_MOTOR_SHIELD"), \
|
#define POLOLU_MOTOR_SHIELD F("POLOLU_MOTOR_SHIELD"), \
|
||||||
|
139
Release_Notes/TrackManager.md
Normal file
139
Release_Notes/TrackManager.md
Normal file
@@ -0,0 +1,139 @@
|
|||||||
|
# DCC++EX Track Manager
|
||||||
|
|
||||||
|
Chris Harlow 2022/03/23
|
||||||
|
|
||||||
|
**If you are only interested in a standard setup using just a DCC track and PROG track, then you DO NOT need to read the rest of this document.**
|
||||||
|
|
||||||
|
What follows is for advanced users interested in managing power districts and/or running DC locomotives through DCC++EX.
|
||||||
|
|
||||||
|
## What is the Track Manager
|
||||||
|
Track Manger (TM from now on) is an integral part of DCC++EX software that is responsible for:
|
||||||
|
- Managing track power state.
|
||||||
|
- Monitoring track overloads and shorts.
|
||||||
|
- Routing the DCC main or prog track waveforms to the correct Motor Driver and thus track.
|
||||||
|
- Managing the JOIN feature.
|
||||||
|
- Intercepting throttle commands to locos running on DC tracks.
|
||||||
|
- Handling user or EXRAIL commands to switch track status.
|
||||||
|
|
||||||
|
In the default scenario of a single DCC track and a PROG track, the TM behaves as for the previous versions of DCC++EX so if thats what you want, you dont need to mess with it.
|
||||||
|
|
||||||
|
The TM is able to handle up to 8 separate track domains. Each domain requires a hardware driver to supply track voltage. A typical motor driver shield supplies two tracks, which is what we have used in the past as main and prog.
|
||||||
|
|
||||||
|
Unlike the previous version of DCC++EX, where the shield channel A was always the DCC main and channel B was always the DCC prog track, TM allows :
|
||||||
|
- None, any or all the tracks can be DCC Main.
|
||||||
|
- None or ONE track may be DCC prog at any given time.
|
||||||
|
- Any track may be powered on or off independently of the others.
|
||||||
|
- Any track may be disconnected from the DCC signal and used as a DC track with a given loco address. (See DC discussion later)
|
||||||
|
|
||||||
|
With such flexibility comes responsibility... the potential for making mistakes means taking extra care with your configuration!
|
||||||
|
|
||||||
|
**NOTE** TM does NOT use "zero stretching" to control your DC motor. Instead, it uses true Pulse Width Modulation (PWM) to efficiently run your loco using the same method a decoder uses to control a DCC loco's motor. DC locos can even run better on TM than they can on a normal analog throttle, especially at low speed, since it is always applying the full track voltage, albeit in pulses of varying duration.
|
||||||
|
|
||||||
|
## Using the Track Manager (DCC)
|
||||||
|
TM names the tracks A to H. In a default setup, you will normally have tracks A and B where A will default to be the DCC main signal and B will be the DCC prog.
|
||||||
|
|
||||||
|
There is a new user command `<=>` which is used to control the TM but the `<0>` and `<1>` commands operate as before.
|
||||||
|
|
||||||
|
- `<=>` lists the current track settings.
|
||||||
|
In a default setup this will normally return
|
||||||
|
```
|
||||||
|
<=A DCC>
|
||||||
|
<=B PROG>
|
||||||
|
```
|
||||||
|
- `<=t DCC>` sets track t (A..H) to use the DCC main track. For example `<=C DCC>` sets track C. All tracks that are set to DCC will receive the same DCC signal waveform.
|
||||||
|
- `<=t PROG>` Sets track t (A..H) to be the one and only PROG track. Any previous PROG track is turned off.
|
||||||
|
- `<=t OFF>` turns off the track t. It will not power on with `<1>` because it will not know what signal to send.
|
||||||
|
|
||||||
|
In an all-DCC environment it is unlikely that you will need to do anything other than setting any additional tracks (C...H) as DCC in your `mySetup.h` file.
|
||||||
|
|
||||||
|
Bear in mind that a track may actually be only connected to DCC accessories such as signals and turnouts... your layout, your choice.
|
||||||
|
|
||||||
|
Note that when setting a track to PROG or OFF, its power is switched off automatically. (The PROG track manages power on an as-needed basis under normal circumstances.
|
||||||
|
When setting a track to MAIN (or DC, DCX see later) the power is applied according to the most recent `<1>` or `<0>` command as being the most compatible with previous versions.
|
||||||
|
|
||||||
|
## using the Track Manager (DC)
|
||||||
|
|
||||||
|
TM allows any or all of your tracks to be individually selected as a DC track which responds to throttle commands on any given loco address. So for example if track A is set to DC address 55, then any throttle commands to loco 55 will be transmitted as DC onto track A and thus a DC loco can be driven along that track. almost exactly as if it was DCC.
|
||||||
|
Your throttle (JMRI, EX-Webthrottle, Withrottle, Engine Driver etc etc) do not know or care that this is a DC loco so nothing needs to change.
|
||||||
|
|
||||||
|
For a simple Command Station setup to run just two DC tracks instead of DCC, you only need to assign DC addresses to tracks A and B. If you want DCC on track A and DC on track B, you just need to set track B to a suitable DC address.
|
||||||
|
|
||||||
|
The command to set a track to a DC address is as follows
|
||||||
|
- `<=t DC a>` Sets track t (A..H) to use loco address a. e.g. <=A DC 3>
|
||||||
|
|
||||||
|
A simple 2 separate loop DC track, wired the traditional way in opposite directions, may be set like this to use loco addresses 1 and 2.
|
||||||
|
```
|
||||||
|
<=A DC 1>
|
||||||
|
<=A DC 2>
|
||||||
|
```
|
||||||
|
|
||||||
|
### Crossing between DC tracks
|
||||||
|
|
||||||
|
There are some slightly mind-bending issues to be addressed, especially if you want to be able to cross between two separate DC tracks or use your layout in DCC or DC mode. This is because the control of DC loco direction is relative to the TRACK and not the LOCO. (you turn a DC loco round on the track and it continues in the same geographical direction. You turn a DCC loco around and it continues to go forwards or backwards in the opposite geographical direction.)
|
||||||
|
|
||||||
|
Generally DC tracks are wired so that two mainline tracks are in opposite direction which makes operation easy BUT crossovers between tracks will cause shorts unless you have very complex switching arrangements.
|
||||||
|
This is generally incompatible with DCC wiring which expects to be able to cross between tracks with impunity because they are all wired with the same polarity.
|
||||||
|
|
||||||
|
To get over this issue TM allows the polarity of a DC track to be swapped so that tracks wired for DCC may be switched to DC with a polarity chosen at run time according to your operations. So, for example, you may have two loops with a crossing between them. Normally you need them in opposite directions, but when you need to drive over the crossing, you need to switch one or other track so that they are at the same polarity.
|
||||||
|
(This is a good case for using EXRAIL to help)
|
||||||
|
|
||||||
|
The command `<=t DCX a>` will set track t (A..H) to be DC but with reversed polarity compared with a track set to DC.
|
||||||
|
|
||||||
|
Its perfectly OK to cross between DC tracks by setting them to the same loco address and making sure you get the polarity right!
|
||||||
|
|
||||||
|
## Connecting Hardware
|
||||||
|
Each track requires hardware to control it
|
||||||
|
- Power on/off
|
||||||
|
- Polarity (direction, signal etc)
|
||||||
|
- Brake (shorts tracks together)
|
||||||
|
- Current (analog reading)
|
||||||
|
|
||||||
|
The standard motor shields provide this for two separate tracks and are predictable and easy to use. However STACKING shields is not a viable way of adding more tracks because it prevents the software from gaining access to the individual track pins. Similarly, wiring all the signal pins together for example, will give you a shared DCC signal but it will eliminate any possibility of switching the track purpose at run time. So, you are going to have to understand enough to wire track drivers to various pins if you wish to extend beyond 2 tracks and take advantage of TM.
|
||||||
|
|
||||||
|
You will also need to consider the implications of differing electronic implementations that would cause unexpected issues when a loco moves between tracks. We know this works fine for a typical shield because we use `<1 JOIN>` quite happily but this may be different if you mix hardware types..... (NOT MY PROBLEM !)
|
||||||
|
|
||||||
|
The easiest way to consider the wiring is to treat each track individually (either as a separate driver or as half of a shield).
|
||||||
|
|
||||||
|
You will require,for each track, on the Arduino:
|
||||||
|
- A GPIO pin (or a HAL vpin perhaps on an I2C extender, code TBA!!!) to switch power.
|
||||||
|
- A GPIO pin to switch the signal direction
|
||||||
|
- A GPIO pin with PWM capability to switch the Brake (you may omit this if you dont want any DC capability)
|
||||||
|
- Optionally An Analog pin to read the current (unless your hardware cant do that, perhaps its just feeding a booster)
|
||||||
|
- Optionally a GPIO fault pin if thats how your hardware works. (NOT recommended as you're going to run out of pins)
|
||||||
|
|
||||||
|
IF you have no more than 3 tracks and you can arrange for the signal pins to be one of 11,12,13 on a Mega, THEN there is a slight advantage internally and the waveform will be super-sharp.
|
||||||
|
|
||||||
|
**Hardware that has two signal pins still needs some code thought!!!!!!!!**
|
||||||
|
|
||||||
|
|
||||||
|
## Configuring the Software
|
||||||
|
|
||||||
|
Configuring the software to provide more tracks is a simple extension of the existing method of customising the #define of MOTOR_SHIELD_TYPE in config.h
|
||||||
|
Since there can be no standard setup of your wiring and hardware choices, it will be necessary to create your custom built MOTOR_SHIELD_TYPE in the manner described in MotorDrivers.h and simply continue to add more `new MotorDriver(` definitions to the list, providing all the pin numbers and electronic limits for each track. (or even shorten the list to 1)
|
||||||
|
|
||||||
|
## Using EXRAIL to control Track Manager
|
||||||
|
EXRAIL has a single additional command that can be used to automate TM.
|
||||||
|
|
||||||
|
- `SET_TRACK(t,mode)`
|
||||||
|
where t is the track letter A..H and mode is one of
|
||||||
|
- `OFF` track is switched off
|
||||||
|
- `DCC` track gets DCC signal
|
||||||
|
- `PROG` track gets DCC prog signal
|
||||||
|
- `DC` track is set to DC mode with the cab address of the currently executing EXRAIL sequence.
|
||||||
|
- `DCX` as DC but with reversed polarity.
|
||||||
|
|
||||||
|
DC/DCX are designed so that you can be automating a DCC loco, drive it onto a separate track and switch to DC without having to know the cab address. (e.g AUTOMATION)
|
||||||
|
If however you are just running a ROUTE you can always do something like this:
|
||||||
|
```
|
||||||
|
ROUTE(77,"Set track G to DC 123")
|
||||||
|
SETLOCO(123)
|
||||||
|
SET_TRACK(G,DC)
|
||||||
|
DONE
|
||||||
|
```
|
||||||
|
|
||||||
|
## Where and How for the Code.
|
||||||
|
The TM code is primarily in TrackManager.cpp which is responsible for coordinating the track settings and commands.
|
||||||
|
|
||||||
|
Each individual track is handled by an instance of MotorDriver created from the MOTOPR_SHIELD_TYPE definition in config.h
|
||||||
|
|
||||||
|
Many functions formerly in the DCCWaveform code have been moved to TrackManager or MotorDriver, notably the power control and checking. This makes the code easier to follow.
|
372
TrackManager.cpp
Normal file
372
TrackManager.cpp
Normal file
@@ -0,0 +1,372 @@
|
|||||||
|
/*
|
||||||
|
* © 2022 Chris Harlow
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* This file is part of DCC++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 "TrackManager.h"
|
||||||
|
#include "FSH.h"
|
||||||
|
#include "DCCWaveform.h"
|
||||||
|
#include "DCC.h"
|
||||||
|
#include "MotorDriver.h"
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
#include "DIAG.h"
|
||||||
|
// Virtualised Motor shield multi-track hardware Interface
|
||||||
|
#define FOR_EACH_TRACK(t) for (byte t=0;t<=lastTrack;t++)
|
||||||
|
|
||||||
|
#define APPLY_BY_MODE(findmode,function) \
|
||||||
|
FOR_EACH_TRACK(t) \
|
||||||
|
if (trackMode[t]==findmode) \
|
||||||
|
track[t]->function;
|
||||||
|
|
||||||
|
const int16_t HASH_KEYWORD_PROG = -29718;
|
||||||
|
const int16_t HASH_KEYWORD_MAIN = 11339;
|
||||||
|
const int16_t HASH_KEYWORD_OFF = 22479;
|
||||||
|
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];
|
||||||
|
TRACK_MODE TrackManager::trackMode[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;
|
||||||
|
int16_t TrackManager::joinRelay=UNUSED_PIN;
|
||||||
|
|
||||||
|
|
||||||
|
// The setup call is done this way so that the tracks can be in a list
|
||||||
|
// from the config... the tracks default to NULL in the declaration
|
||||||
|
void TrackManager::Setup(const FSH * shieldname,
|
||||||
|
MotorDriver * track0, MotorDriver * track1, MotorDriver * track2,
|
||||||
|
MotorDriver * track3, MotorDriver * track4, MotorDriver * track5,
|
||||||
|
MotorDriver * track6, MotorDriver * track7 ) {
|
||||||
|
addTrack(0,track0);
|
||||||
|
addTrack(1,track1);
|
||||||
|
addTrack(2,track2);
|
||||||
|
addTrack(3,track3);
|
||||||
|
addTrack(4,track4);
|
||||||
|
addTrack(5,track5);
|
||||||
|
addTrack(6,track6);
|
||||||
|
addTrack(7,track7);
|
||||||
|
|
||||||
|
// Default the first 2 tracks (which may be null) and perform HA waveform check.
|
||||||
|
setTrackMode(0,TRACK_MODE_MAIN);
|
||||||
|
setTrackMode(1,TRACK_MODE_PROG);
|
||||||
|
|
||||||
|
// TODO Fault pin config for odd motor boards (example pololu)
|
||||||
|
// MotorDriver::commonFaultPin = ((mainDriver->getFaultPin() == progDriver->getFaultPin())
|
||||||
|
// && (mainDriver->getFaultPin() != UNUSED_PIN));
|
||||||
|
DIAG(F("Signal pin config: %S accuracy waveform"),
|
||||||
|
MotorDriver::usePWM ? F("high") : F("normal") );
|
||||||
|
DCC::begin(shieldname);
|
||||||
|
}
|
||||||
|
|
||||||
|
void TrackManager::addTrack(byte t, MotorDriver* driver) {
|
||||||
|
trackMode[t]=TRACK_MODE_OFF;
|
||||||
|
track[t]=driver;
|
||||||
|
if (driver) {
|
||||||
|
track[t]->setPower(POWERMODE::OFF);
|
||||||
|
lastTrack=t;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// The port registers that are shadowing
|
||||||
|
// the real port registers. These are
|
||||||
|
// defined in Motordriver.cpp
|
||||||
|
extern byte fakePORTA;
|
||||||
|
extern byte fakePORTB;
|
||||||
|
extern byte fakePORTC;
|
||||||
|
|
||||||
|
// setDCCSignal(), called from interrupt context
|
||||||
|
// does assume ports are shadowed if they can be
|
||||||
|
void TrackManager::setDCCSignal( bool on) {
|
||||||
|
HAVE_PORTA(fakePORTA=PORTA);
|
||||||
|
HAVE_PORTB(fakePORTB=PORTB);
|
||||||
|
HAVE_PORTC(fakePORTC=PORTC);
|
||||||
|
APPLY_BY_MODE(TRACK_MODE_MAIN,setSignal(on));
|
||||||
|
HAVE_PORTA(PORTA=fakePORTA);
|
||||||
|
HAVE_PORTB(PORTB=fakePORTB);
|
||||||
|
HAVE_PORTC(PORTC=fakePORTC);
|
||||||
|
}
|
||||||
|
|
||||||
|
void TrackManager::setCutout( bool on) {
|
||||||
|
(void) on;
|
||||||
|
// TODO Cutout needs fake ports as well
|
||||||
|
// TODO APPLY_BY_MODE(TRACK_MODE_MAIN,setCutout(on));
|
||||||
|
}
|
||||||
|
|
||||||
|
// setPROGSignal(), called from interrupt context
|
||||||
|
// does assume ports are shadowed if they can be
|
||||||
|
void TrackManager::setPROGSignal( bool on) {
|
||||||
|
HAVE_PORTA(fakePORTA=PORTA);
|
||||||
|
HAVE_PORTB(fakePORTB=PORTB);
|
||||||
|
HAVE_PORTC(fakePORTC=PORTC);
|
||||||
|
APPLY_BY_MODE(TRACK_MODE_PROG,setSignal(on));
|
||||||
|
HAVE_PORTA(PORTA=fakePORTA);
|
||||||
|
HAVE_PORTB(PORTB=fakePORTB);
|
||||||
|
HAVE_PORTC(PORTC=fakePORTC);
|
||||||
|
}
|
||||||
|
|
||||||
|
// setDCSignal(), called from normal context
|
||||||
|
// MotorDriver::setDCSignal handles shadowed IO port changes.
|
||||||
|
// with interrupts turned off around the critical section
|
||||||
|
void TrackManager::setDCSignal(int16_t cab, byte speedbyte) {
|
||||||
|
FOR_EACH_TRACK(t) {
|
||||||
|
if (trackDCAddr[t]!=cab) continue;
|
||||||
|
if (trackMode[t]==TRACK_MODE_DC) track[t]->setDCSignal(speedbyte);
|
||||||
|
else if (trackMode[t]==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;
|
||||||
|
|
||||||
|
DIAG(F("Track=%c"),trackToSet+'A');
|
||||||
|
// DC tracks require a motorDriver that can set brake!
|
||||||
|
if ((mode==TRACK_MODE_DC || mode==TRACK_MODE_DCX)
|
||||||
|
&& !track[trackToSet]->brakeCanPWM()) {
|
||||||
|
DIAG(F("Brake pin can't PWM: No DC"));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mode==TRACK_MODE_PROG) {
|
||||||
|
// only allow 1 track to be prog
|
||||||
|
FOR_EACH_TRACK(t)
|
||||||
|
if (trackMode[t]==TRACK_MODE_PROG && t != trackToSet) {
|
||||||
|
track[t]->setPower(POWERMODE::OFF);
|
||||||
|
trackMode[t]=TRACK_MODE_OFF;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
track[trackToSet]->setResetCounterPointer(NULL); // only the prog track has this pointer set
|
||||||
|
}
|
||||||
|
trackMode[trackToSet]=mode;
|
||||||
|
trackDCAddr[trackToSet]=dcAddr;
|
||||||
|
|
||||||
|
// When a track is switched, we must clear any side effects of its previous
|
||||||
|
// state, otherwise trains run away or just dont move.
|
||||||
|
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);
|
||||||
|
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// DCC tracks need to have set the PWM to zero or they will not work.
|
||||||
|
// 128 is speed=0 and dir=0 and then loosen brake.
|
||||||
|
track[trackToSet]->setDCSignal(128);
|
||||||
|
track[trackToSet]->setBrake(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// re-evaluate HighAccuracy mode
|
||||||
|
// We can only do this is all main and prog tracks agree
|
||||||
|
bool canDo=true;
|
||||||
|
FOR_EACH_TRACK(t) {
|
||||||
|
// 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 (trackMode[t]==TRACK_MODE_DC || trackMode[t]==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
|
||||||
|
} else {
|
||||||
|
track[t]->trackPWM=false; // this track sure can not run with PWM
|
||||||
|
DIAG(F("Track %c trackPWM 0 (not capable)"), t+'A');
|
||||||
|
}
|
||||||
|
} else if (trackMode[t]==TRACK_MODE_MAIN || trackMode[t]==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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!canDo) {
|
||||||
|
// if we discover that HA mode was globally impossible
|
||||||
|
// we must adjust the trackPWM capabilities
|
||||||
|
FOR_EACH_TRACK(t) {
|
||||||
|
track[t]->trackPWM=false;
|
||||||
|
DIAG(F("Track %c trackPWM 0 (global override)"), t+'A');
|
||||||
|
}
|
||||||
|
DCCTimer::clearPWM(); // has to be AFTER trackPWM changes because if trackPWM==true this is undone for that track
|
||||||
|
}
|
||||||
|
if (MotorDriver::usePWM != canDo)
|
||||||
|
DIAG(F("HA mode changed from %d to %d"), MotorDriver::usePWM, canDo);
|
||||||
|
MotorDriver::usePWM=canDo;
|
||||||
|
|
||||||
|
|
||||||
|
// 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 (trackMode[t]==TRACK_MODE_DCX)
|
||||||
|
speedByte = speedByte ^ 128; // reverse direction bit
|
||||||
|
track[t]->setDCSignal(speedByte);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TrackManager::parseJ(Print *stream, int16_t params, int16_t p[])
|
||||||
|
{
|
||||||
|
|
||||||
|
if (params==0) { // <=> List track assignments
|
||||||
|
FOR_EACH_TRACK(t)
|
||||||
|
if (track[t]!=NULL) {
|
||||||
|
StringFormatter::send(stream,F("<= %c "),'A'+t);
|
||||||
|
switch(trackMode[t]) {
|
||||||
|
case TRACK_MODE_MAIN:
|
||||||
|
StringFormatter::send(stream,F("MAIN"));
|
||||||
|
break;
|
||||||
|
case TRACK_MODE_PROG:
|
||||||
|
StringFormatter::send(stream,F("PROG"));
|
||||||
|
break;
|
||||||
|
case TRACK_MODE_OFF:
|
||||||
|
StringFormatter::send(stream,F("OFF"));
|
||||||
|
break;
|
||||||
|
case TRACK_MODE_EXT:
|
||||||
|
StringFormatter::send(stream,F("EXT"));
|
||||||
|
break;
|
||||||
|
case TRACK_MODE_DC:
|
||||||
|
StringFormatter::send(stream,F("DC %d"),trackDCAddr[t]);
|
||||||
|
break;
|
||||||
|
case TRACK_MODE_DCX:
|
||||||
|
StringFormatter::send(stream,F("DCX %d"),trackDCAddr[t]);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break; // unknown, dont care
|
||||||
|
}
|
||||||
|
StringFormatter::send(stream,F(">\n"));
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
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]==HASH_KEYWORD_MAIN) // <= id MAIN>
|
||||||
|
return setTrackMode(p[0],TRACK_MODE_MAIN);
|
||||||
|
|
||||||
|
if (params==2 && p[1]==HASH_KEYWORD_PROG) // <= id PROG>
|
||||||
|
return setTrackMode(p[0],TRACK_MODE_PROG);
|
||||||
|
|
||||||
|
if (params==2 && p[1]==HASH_KEYWORD_OFF) // <= id OFF>
|
||||||
|
return setTrackMode(p[0],TRACK_MODE_OFF);
|
||||||
|
|
||||||
|
if (params==2 && p[1]==HASH_KEYWORD_EXT) // <= id EXT>
|
||||||
|
return setTrackMode(p[0],TRACK_MODE_EXT);
|
||||||
|
|
||||||
|
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]==HASH_KEYWORD_DCX && p[2]>0) // <= id DCX cab>
|
||||||
|
return setTrackMode(p[0],TRACK_MODE_DCX,p[2]);
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
byte TrackManager::nextCycleTrack=MAX_TRACKS;
|
||||||
|
|
||||||
|
void TrackManager::loop() {
|
||||||
|
DCCWaveform::loop();
|
||||||
|
DCCACK::loop();
|
||||||
|
bool dontLimitProg=DCCACK::isActive() || progTrackSyncMain || progTrackBoosted;
|
||||||
|
nextCycleTrack++;
|
||||||
|
if (nextCycleTrack>lastTrack) nextCycleTrack=0;
|
||||||
|
if (track[nextCycleTrack]==NULL) return;
|
||||||
|
MotorDriver * motorDriver=track[nextCycleTrack];
|
||||||
|
bool useProgLimit=dontLimitProg? false: trackMode[nextCycleTrack]==TRACK_MODE_PROG;
|
||||||
|
motorDriver->checkPowerOverload(useProgLimit, nextCycleTrack);
|
||||||
|
}
|
||||||
|
|
||||||
|
MotorDriver * TrackManager::getProgDriver() {
|
||||||
|
FOR_EACH_TRACK(t)
|
||||||
|
if (trackMode[t]==TRACK_MODE_PROG) return track[t];
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
void TrackManager::setPower2(bool setProg,POWERMODE mode) {
|
||||||
|
if (!setProg) mainPowerGuess=mode;
|
||||||
|
FOR_EACH_TRACK(t) {
|
||||||
|
MotorDriver * driver=track[t];
|
||||||
|
if (!driver) continue;
|
||||||
|
switch (trackMode[t]) {
|
||||||
|
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:
|
||||||
|
if (setProg) break;
|
||||||
|
driver->setBrake(true); // DC starts with brake on
|
||||||
|
applyDCSpeed(t); // speed match DCC throttles
|
||||||
|
driver->setPower(mode);
|
||||||
|
break;
|
||||||
|
case TRACK_MODE_PROG:
|
||||||
|
if (!setProg) 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_OFF:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
POWERMODE TrackManager::getProgPower() {
|
||||||
|
FOR_EACH_TRACK(t)
|
||||||
|
if (trackMode[t]==TRACK_MODE_PROG)
|
||||||
|
return track[t]->getPower();
|
||||||
|
return POWERMODE::OFF;
|
||||||
|
}
|
||||||
|
|
||||||
|
void TrackManager::setJoinRelayPin(byte joinRelayPin) {
|
||||||
|
joinRelay=joinRelayPin;
|
||||||
|
if (joinRelay!=UNUSED_PIN) {
|
||||||
|
pinMode(joinRelay,OUTPUT);
|
||||||
|
digitalWrite(joinRelay,LOW); // LOW is relay disengaged
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void TrackManager::setJoin(bool joined) {
|
||||||
|
progTrackSyncMain=joined;
|
||||||
|
if (joinRelay!=UNUSED_PIN) digitalWrite(joinRelay,joined?HIGH:LOW);
|
||||||
|
}
|
88
TrackManager.h
Normal file
88
TrackManager.h
Normal file
@@ -0,0 +1,88 @@
|
|||||||
|
/*
|
||||||
|
* © 2022 Chris Harlow
|
||||||
|
* 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/>.
|
||||||
|
*/
|
||||||
|
#ifndef TrackManager_h
|
||||||
|
#define TrackManager_h
|
||||||
|
#include "FSH.h"
|
||||||
|
#include "MotorDriver.h"
|
||||||
|
// Virtualised Motor shield multi-track hardware Interface
|
||||||
|
|
||||||
|
enum TRACK_MODE : byte {TRACK_MODE_OFF, TRACK_MODE_MAIN, TRACK_MODE_PROG,
|
||||||
|
TRACK_MODE_DC, TRACK_MODE_DCX, TRACK_MODE_EXT};
|
||||||
|
|
||||||
|
// These constants help EXRAIL macros say SET_TRACK(2,mode) OR SET_TRACK(C,mode) etc.
|
||||||
|
const byte TRACK_NUMBER_0=0, TRACK_NUMBER_A=0;
|
||||||
|
const byte TRACK_NUMBER_1=1, TRACK_NUMBER_B=1;
|
||||||
|
const byte TRACK_NUMBER_2=2, TRACK_NUMBER_C=2;
|
||||||
|
const byte TRACK_NUMBER_3=3, TRACK_NUMBER_D=3;
|
||||||
|
const byte TRACK_NUMBER_4=4, TRACK_NUMBER_E=4;
|
||||||
|
const byte TRACK_NUMBER_5=5, TRACK_NUMBER_F=5;
|
||||||
|
const byte TRACK_NUMBER_6=6, TRACK_NUMBER_G=6;
|
||||||
|
const byte TRACK_NUMBER_7=7, TRACK_NUMBER_H=7;
|
||||||
|
|
||||||
|
class TrackManager {
|
||||||
|
public:
|
||||||
|
static void Setup(const FSH * shieldName,
|
||||||
|
MotorDriver * track0,
|
||||||
|
MotorDriver * track1=NULL,
|
||||||
|
MotorDriver * track2=NULL,
|
||||||
|
MotorDriver * track3=NULL,
|
||||||
|
MotorDriver * track4=NULL,
|
||||||
|
MotorDriver * track5=NULL,
|
||||||
|
MotorDriver * track6=NULL,
|
||||||
|
MotorDriver * track7=NULL
|
||||||
|
);
|
||||||
|
|
||||||
|
static void setDCCSignal( bool on);
|
||||||
|
static void setCutout( bool on);
|
||||||
|
static void setPROGSignal( bool on);
|
||||||
|
static void setDCSignal(int16_t cab, byte speedbyte);
|
||||||
|
static MotorDriver * getProgDriver();
|
||||||
|
static void setPower2(bool progTrack,POWERMODE mode);
|
||||||
|
static void setPower(POWERMODE mode) {setMainPower(mode); setProgPower(mode);}
|
||||||
|
static void setMainPower(POWERMODE mode) {setPower2(false,mode);}
|
||||||
|
static void setProgPower(POWERMODE mode) {setPower2(true,mode);}
|
||||||
|
|
||||||
|
static const int16_t MAX_TRACKS=8;
|
||||||
|
static bool setTrackMode(byte track, TRACK_MODE mode, int16_t DCaddr=0);
|
||||||
|
static bool parseJ(Print * stream, int16_t params, int16_t p[]);
|
||||||
|
static void loop();
|
||||||
|
static POWERMODE getMainPower() {return mainPowerGuess;}
|
||||||
|
static POWERMODE getProgPower();
|
||||||
|
static void setJoin(bool join);
|
||||||
|
static bool isJoined() { return progTrackSyncMain;}
|
||||||
|
static void setJoinRelayPin(byte joinRelayPin);
|
||||||
|
static int16_t joinRelay;
|
||||||
|
static bool progTrackSyncMain; // true when prog track is a siding switched to main
|
||||||
|
static bool progTrackBoosted; // true when prog track is not current limited
|
||||||
|
|
||||||
|
|
||||||
|
private:
|
||||||
|
static void addTrack(byte t, MotorDriver* driver);
|
||||||
|
static byte lastTrack;
|
||||||
|
static byte nextCycleTrack;
|
||||||
|
static POWERMODE mainPowerGuess;
|
||||||
|
static void applyDCSpeed(byte t);
|
||||||
|
|
||||||
|
static MotorDriver* track[MAX_TRACKS];
|
||||||
|
static TRACK_MODE trackMode[MAX_TRACKS];
|
||||||
|
static int16_t trackDCAddr[MAX_TRACKS]; // dc address if TRACK_MODE_DC or TRACK_MODE_DCX
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
@@ -55,6 +55,8 @@
|
|||||||
#include "version.h"
|
#include "version.h"
|
||||||
#include "EXRAIL2.h"
|
#include "EXRAIL2.h"
|
||||||
#include "CommandDistributor.h"
|
#include "CommandDistributor.h"
|
||||||
|
#include "TrackManager.h"
|
||||||
|
#include "DCCTimer.h"
|
||||||
|
|
||||||
#define LOOPLOCOS(THROTTLECHAR, CAB) for (int loco=0;loco<MAX_MY_LOCO;loco++) \
|
#define LOOPLOCOS(THROTTLECHAR, CAB) for (int loco=0;loco<MAX_MY_LOCO;loco++) \
|
||||||
if ((myLocos[loco].throttle==THROTTLECHAR || '*'==THROTTLECHAR) && (CAB<0 || myLocos[loco].cab==CAB))
|
if ((myLocos[loco].throttle==THROTTLECHAR || '*'==THROTTLECHAR) && (CAB<0 || myLocos[loco].cab==CAB))
|
||||||
@@ -165,9 +167,12 @@ void WiThrottle::parse(RingStream * stream, byte * cmdx) {
|
|||||||
break;
|
break;
|
||||||
case 'P':
|
case 'P':
|
||||||
if (cmd[1]=='P' && cmd[2]=='A' ) { //PPA power mode
|
if (cmd[1]=='P' && cmd[2]=='A' ) { //PPA power mode
|
||||||
DCCWaveform::mainTrack.setPowerMode(cmd[3]=='1'?POWERMODE::ON:POWERMODE::OFF);
|
TrackManager::setMainPower(cmd[3]=='1'?POWERMODE::ON:POWERMODE::OFF);
|
||||||
|
/* TODO
|
||||||
if (MotorDriver::commonFaultPin) // commonFaultPin prevents individual track handling
|
if (MotorDriver::commonFaultPin) // commonFaultPin prevents individual track handling
|
||||||
DCCWaveform::progTrack.setPowerMode(cmd[3]=='1'?POWERMODE::ON:POWERMODE::OFF);
|
DCCWaveform::progTrack.setPowerMode(cmd[3]=='1'?POWERMODE::ON:POWERMODE::OFF);
|
||||||
|
*/
|
||||||
|
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
}
|
}
|
||||||
#if defined(EXRAIL_ACTIVE)
|
#if defined(EXRAIL_ACTIVE)
|
||||||
@@ -218,9 +223,7 @@ void WiThrottle::parse(RingStream * stream, byte * cmdx) {
|
|||||||
StringFormatter::send(stream,F("VN2.0\nHTDCC-EX\nRL0\n"));
|
StringFormatter::send(stream,F("VN2.0\nHTDCC-EX\nRL0\n"));
|
||||||
StringFormatter::send(stream,F("HtDCC-EX v%S, %S, %S, %S\n"), F(VERSION), F(ARDUINO_TYPE), DCC::getMotorShieldName(), F(GITHUB_SHA));
|
StringFormatter::send(stream,F("HtDCC-EX v%S, %S, %S, %S\n"), F(VERSION), F(ARDUINO_TYPE), DCC::getMotorShieldName(), F(GITHUB_SHA));
|
||||||
StringFormatter::send(stream,F("PTT]\\[Turnouts}|{Turnout]\\[THROW}|{2]\\[CLOSE}|{4\n"));
|
StringFormatter::send(stream,F("PTT]\\[Turnouts}|{Turnout]\\[THROW}|{2]\\[CLOSE}|{4\n"));
|
||||||
StringFormatter::send(stream,F("PPA%x\n"),DCCWaveform::mainTrack.getPowerMode()==POWERMODE::ON);
|
StringFormatter::send(stream,F("PPA%x\n"),TrackManager::getMainPower()==POWERMODE::ON);
|
||||||
|
|
||||||
// Send the roster
|
|
||||||
#ifdef EXRAIL_ACTIVE
|
#ifdef EXRAIL_ACTIVE
|
||||||
StringFormatter::send(stream,F("RL%d"), RMFT2::rosterNameCount);
|
StringFormatter::send(stream,F("RL%d"), RMFT2::rosterNameCount);
|
||||||
for (int16_t r=0;r<RMFT2::rosterNameCount;r++) {
|
for (int16_t r=0;r<RMFT2::rosterNameCount;r++) {
|
||||||
@@ -418,7 +421,10 @@ void WiThrottle::locoAction(RingStream * stream, byte* aval, char throttleChar,
|
|||||||
bool forward=aval[1]!='0';
|
bool forward=aval[1]!='0';
|
||||||
LOOPLOCOS(throttleChar, cab) {
|
LOOPLOCOS(throttleChar, cab) {
|
||||||
mostRecentCab=myLocos[loco].cab;
|
mostRecentCab=myLocos[loco].cab;
|
||||||
DCC::setThrottle(myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab), forward);
|
int8_t speed = DCC::getThrottleSpeed(myLocos[loco].cab);
|
||||||
|
if (speed < 0) //can not find any speed for this cab
|
||||||
|
speed = 0;
|
||||||
|
DCC::setThrottle(myLocos[loco].cab, speed, forward);
|
||||||
// setThrottle will cause a broadcast so notification will be sent
|
// setThrottle will cause a broadcast so notification will be sent
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -564,8 +570,8 @@ void WiThrottle::getLocoCallback(int16_t locoid) {
|
|||||||
char addcmd[20]={'M',stashThrottleChar,'+', addrchar};
|
char addcmd[20]={'M',stashThrottleChar,'+', addrchar};
|
||||||
itoa(locoid,addcmd+4,10);
|
itoa(locoid,addcmd+4,10);
|
||||||
stashInstance->multithrottle(stashStream, (byte *)addcmd);
|
stashInstance->multithrottle(stashStream, (byte *)addcmd);
|
||||||
DCCWaveform::progTrack.setPowerMode(POWERMODE::ON);
|
TrackManager::setMainPower(POWERMODE::ON);
|
||||||
DCC::setProgTrackSyncMain(true); // <1 JOIN> so we can drive loco away
|
TrackManager::setJoin(true); // <1 JOIN> so we can drive loco away
|
||||||
stashStream->commit();
|
stashStream->commit();
|
||||||
CommandDistributor::broadcastPower();
|
CommandDistributor::broadcastPower();
|
||||||
|
|
||||||
|
@@ -22,7 +22,7 @@
|
|||||||
#ifndef ARDUINO_AVR_UNO_WIFI_REV2
|
#ifndef ARDUINO_AVR_UNO_WIFI_REV2
|
||||||
// This code is NOT compiled on a unoWifiRev2 processor which uses a different architecture
|
// This code is NOT compiled on a unoWifiRev2 processor which uses a different architecture
|
||||||
#include "WifiInterface.h" /* config.h included there */
|
#include "WifiInterface.h" /* config.h included there */
|
||||||
#include <avr/pgmspace.h>
|
//#include <avr/pgmspace.h>
|
||||||
#include "DIAG.h"
|
#include "DIAG.h"
|
||||||
#include "StringFormatter.h"
|
#include "StringFormatter.h"
|
||||||
|
|
||||||
|
@@ -23,7 +23,7 @@
|
|||||||
#include "FSH.h"
|
#include "FSH.h"
|
||||||
#include "DCCEXParser.h"
|
#include "DCCEXParser.h"
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include <avr/pgmspace.h>
|
//#include <avr/pgmspace.h>
|
||||||
|
|
||||||
enum wifiSerialState { WIFI_NOAT, WIFI_DISCONNECTED, WIFI_CONNECTED };
|
enum wifiSerialState { WIFI_NOAT, WIFI_DISCONNECTED, WIFI_CONNECTED };
|
||||||
|
|
||||||
|
@@ -40,7 +40,7 @@
|
|||||||
// WIFI_ON: All prereqs for running with WIFI are met
|
// WIFI_ON: All prereqs for running with WIFI are met
|
||||||
// Note: WIFI_CHANNEL may not exist in early config.h files so is added here if needed.
|
// Note: WIFI_CHANNEL may not exist in early config.h files so is added here if needed.
|
||||||
|
|
||||||
#if (defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_SAMD_ZERO) || defined(TEENSYDUINO)) || defined(ARDUINO_AVR_NANO_EVERY)
|
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_SAMD_ZERO) || defined(TEENSYDUINO) || defined(ARDUINO_AVR_NANO_EVERY) || defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)
|
||||||
#define BIG_RAM
|
#define BIG_RAM
|
||||||
#endif
|
#endif
|
||||||
#if ENABLE_WIFI
|
#if ENABLE_WIFI
|
||||||
|
112
freeMemory.cpp
112
freeMemory.cpp
@@ -1,112 +0,0 @@
|
|||||||
/*
|
|
||||||
* © 2021 Neil McKechnie
|
|
||||||
* © 2021 Mike S
|
|
||||||
* © 2020 Harald Barth
|
|
||||||
*
|
|
||||||
* This file is part of Asbelos DCC-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 <Arduino.h>
|
|
||||||
#include "freeMemory.h"
|
|
||||||
|
|
||||||
// thanks go to https://github.com/mpflaga/Arduino-MemoryFree
|
|
||||||
#if defined(__arm__)
|
|
||||||
extern "C" char* sbrk(int);
|
|
||||||
#elif defined(__AVR__)
|
|
||||||
extern char *__brkval;
|
|
||||||
extern char *__malloc_heap_start;
|
|
||||||
#else
|
|
||||||
#error Unsupported board type
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
static volatile int minimum_free_memory = __INT_MAX__;
|
|
||||||
|
|
||||||
#if !defined(__IMXRT1062__)
|
|
||||||
static inline int freeMemory() {
|
|
||||||
char top;
|
|
||||||
#if defined(__arm__)
|
|
||||||
return &top - reinterpret_cast<char*>(sbrk(0));
|
|
||||||
#elif defined(__AVR__)
|
|
||||||
return __brkval ? &top - __brkval : &top - __malloc_heap_start;
|
|
||||||
#else
|
|
||||||
#error bailed out already above
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return low memory value.
|
|
||||||
int minimumFreeMemory() {
|
|
||||||
byte sreg_save = SREG;
|
|
||||||
noInterrupts(); // Disable interrupts
|
|
||||||
int retval = minimum_free_memory;
|
|
||||||
SREG = sreg_save; // Restore interrupt state
|
|
||||||
return retval;
|
|
||||||
}
|
|
||||||
|
|
||||||
#else
|
|
||||||
#if defined(ARDUINO_TEENSY40)
|
|
||||||
static const unsigned DTCM_START = 0x20000000UL;
|
|
||||||
static const unsigned OCRAM_START = 0x20200000UL;
|
|
||||||
static const unsigned OCRAM_SIZE = 512;
|
|
||||||
static const unsigned FLASH_SIZE = 1984;
|
|
||||||
#elif defined(ARDUINO_TEENSY41)
|
|
||||||
static const unsigned DTCM_START = 0x20000000UL;
|
|
||||||
static const unsigned OCRAM_START = 0x20200000UL;
|
|
||||||
static const unsigned OCRAM_SIZE = 512;
|
|
||||||
static const unsigned FLASH_SIZE = 7936;
|
|
||||||
#if TEENSYDUINO>151
|
|
||||||
extern "C" uint8_t external_psram_size;
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
static inline int freeMemory() {
|
|
||||||
extern unsigned long _ebss;
|
|
||||||
extern unsigned long _sdata;
|
|
||||||
extern unsigned long _estack;
|
|
||||||
const unsigned DTCM_START = 0x20000000UL;
|
|
||||||
unsigned dtcm = (unsigned)&_estack - DTCM_START;
|
|
||||||
unsigned stackinuse = (unsigned) &_estack - (unsigned) __builtin_frame_address(0);
|
|
||||||
unsigned varsinuse = (unsigned)&_ebss - (unsigned)&_sdata;
|
|
||||||
unsigned freemem = dtcm - (stackinuse + varsinuse);
|
|
||||||
return freemem;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Return low memory value.
|
|
||||||
int minimumFreeMemory() {
|
|
||||||
//byte sreg_save = SREG;
|
|
||||||
//noInterrupts(); // Disable interrupts
|
|
||||||
int retval = minimum_free_memory;
|
|
||||||
//SREG = sreg_save; // Restore interrupt state
|
|
||||||
return retval;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
// Update low ram level. Allow for extra bytes to be specified
|
|
||||||
// by estimation or inspection, that may be used by other
|
|
||||||
// called subroutines. Must be called with interrupts disabled.
|
|
||||||
//
|
|
||||||
// Although __brkval may go up and down as heap memory is allocated
|
|
||||||
// and freed, this function records only the worst case encountered.
|
|
||||||
// So even if all of the heap is freed, the reported minimum free
|
|
||||||
// memory will not increase.
|
|
||||||
//
|
|
||||||
void updateMinimumFreeMemory(unsigned char extraBytes) {
|
|
||||||
int spare = freeMemory()-extraBytes;
|
|
||||||
if (spare < 0) spare = 0;
|
|
||||||
if (spare < minimum_free_memory) minimum_free_memory = spare;
|
|
||||||
}
|
|
||||||
|
|
25
freeMemory.h
25
freeMemory.h
@@ -1,25 +0,0 @@
|
|||||||
/*
|
|
||||||
* © 2021 Neil McKechnie
|
|
||||||
* © 2020 Harald Barth
|
|
||||||
*
|
|
||||||
* This file is part of DCC-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 freeMemory_h
|
|
||||||
#define freeMemory_h
|
|
||||||
void updateMinimumFreeMemory(unsigned char extraBytes=0);
|
|
||||||
int minimumFreeMemory();
|
|
||||||
#endif
|
|
@@ -4,7 +4,13 @@
|
|||||||
#include "StringFormatter.h"
|
#include "StringFormatter.h"
|
||||||
|
|
||||||
|
|
||||||
#define VERSION "4.0.2"
|
#define VERSION "4.0.3"
|
||||||
|
// 4.0.3 Track Manager additions:
|
||||||
|
// SET_TRACK(track,mode) Functions (A-H, MAIN|PROG|DC|DCX|OFF)
|
||||||
|
// New DC track function and DCX reverse polarity function
|
||||||
|
// TrackManager DCC & DC up to 8 Districts Architecture
|
||||||
|
// Automatic ALIAS(name)
|
||||||
|
// Command Parser now accepts Underscore in Alias Names
|
||||||
// 4.0.2 EXRAIL additions:
|
// 4.0.2 EXRAIL additions:
|
||||||
// ACK defaults set to 50mA LIMIT, 2000uS MIN, 20000uS MAX
|
// ACK defaults set to 50mA LIMIT, 2000uS MIN, 20000uS MAX
|
||||||
// myFilter automatic detection (no need to call setFilter)
|
// myFilter automatic detection (no need to call setFilter)
|
||||||
|
Reference in New Issue
Block a user