mirror of
https://github.com/DCC-EX/CommandStation-EX.git
synced 2024-11-26 17:46:14 +01:00
Railcom timing
This commit is contained in:
parent
dc481a2f0c
commit
61c8f6b047
|
@ -57,66 +57,59 @@ void DCCTimer::begin(INTERRUPT_CALLBACK callback) {
|
||||||
TCCR1B = _BV(WGM13) | _BV(CS10); // Mode 8, clock select 1
|
TCCR1B = _BV(WGM13) | _BV(CS10); // Mode 8, clock select 1
|
||||||
TIMSK1 = _BV(TOIE1); // Enable Software interrupt
|
TIMSK1 = _BV(TOIE1); // Enable Software interrupt
|
||||||
interrupts();
|
interrupts();
|
||||||
|
//diagnostic pinMode(4,OUTPUT);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void DCCTimer::startRailcomTimer(byte brakePin) {
|
void DCCTimer::startRailcomTimer(byte brakePin) {
|
||||||
|
(void) brakePin; // Ignored... works on pin 9 only
|
||||||
|
// diagnostic digitalWrite(4,HIGH);
|
||||||
|
|
||||||
/* The Railcom timer is started in such a way that it
|
/* The Railcom timer is started in such a way that it
|
||||||
- First triggers 28uS after the last TIMER1 tick.
|
- First triggers 58+29 uS after the previous TIMER1 tick.
|
||||||
This provides an accurate offset (in High Accuracy mode)
|
This provides an accurate offset (in High Accuracy mode)
|
||||||
for the start of the Railcom cutout.
|
for the start of the Railcom cutout.
|
||||||
- Sets the Railcom pin high at first tick,
|
- Sets the Railcom pin high at first tick and subsequent ticks
|
||||||
because its been setup with 100% PWM duty cycle.
|
until its reset to setting pin 9 low at next tick.
|
||||||
|
|
||||||
- Cycles at 436uS so the second tick is the
|
- Cycles at 436uS so the second tick is the
|
||||||
correct distance from the cutout.
|
correct distance from the cutout.
|
||||||
|
|
||||||
- Waveform code is responsible for altering the PWM
|
- Waveform code is responsible for resetting
|
||||||
duty cycle to 0% any time between the first and last tick.
|
any time between the first and second tick.
|
||||||
(there will be 7 DCC timer1 ticks in which to do this.)
|
(there will be 7 DCC timer1 ticks in which to do this.)
|
||||||
|
|
||||||
*/
|
*/
|
||||||
(void) brakePin; // Ignored... works on pin 9 only
|
|
||||||
const int cutoutDuration = 430; // Desired interval in microseconds
|
const int cutoutDuration = 430; // Desired interval in microseconds
|
||||||
|
const int cycle=cutoutDuration/2;
|
||||||
// Set up Timer2 for CTC mode (Clear Timer on Compare Match)
|
|
||||||
TCCR2A = 0; // Clear Timer2 control register A
|
|
||||||
TCCR2B = 0; // Clear Timer2 control register B
|
|
||||||
TCNT2 = 0; // Initialize Timer2 counter value to 0
|
|
||||||
// Configure Phase and Frequency Correct PWM mode
|
|
||||||
TCCR2A = (1 << COM2B1); // enable pwm on pin 9
|
|
||||||
TCCR2A |= (1 << WGM20);
|
|
||||||
|
|
||||||
|
|
||||||
// Set Timer 2 prescaler to 32
|
const byte RailcomFudge0=58+58+29;
|
||||||
TCCR2B = (1 << CS21) | (1 << CS20); // 32 prescaler
|
|
||||||
|
// Set Timer2 to CTC mode with set on compare match
|
||||||
// Set the compare match value for desired interval
|
TCCR2A = (1 << WGM21) | (1 << COM2B0) | (1 << COM2B1);
|
||||||
OCR2A = (F_CPU / 1000000) * cutoutDuration / 64 - 1;
|
// Prescaler of 32
|
||||||
|
TCCR2B = (1 << CS21) | (1 << CS20);
|
||||||
// Calculate the compare match value for desired duty cycle
|
OCR2A = cycle-1; // Compare match value for 430 uS
|
||||||
OCR2B = OCR2A+1; // set duty cycle to 100%= OCR2A)
|
|
||||||
|
|
||||||
// Enable Timer2 output on pin 9 (OC2B)
|
// Enable Timer2 output on pin 9 (OC2B)
|
||||||
DDRB |= (1 << DDB1);
|
DDRB |= (1 << DDB1);
|
||||||
// TODO Fudge TCNT2 to sync with last tcnt1 tick + 28uS
|
|
||||||
|
|
||||||
|
// RailcomFudge2 is the expected time from idealised
|
||||||
|
// setup call (at previous DCC timer interrupt) to the cutout.
|
||||||
|
// This value should be reduced to reflect the Timer1 value
|
||||||
|
// measuring the time since the previous hardware interrupt
|
||||||
|
byte tcfudge=TCNT1/16;
|
||||||
|
TCNT2=cycle-RailcomFudge0/2+tcfudge/2;
|
||||||
|
|
||||||
|
|
||||||
// Previous TIMER1 Tick was at rising end-of-packet bit
|
// Previous TIMER1 Tick was at rising end-of-packet bit
|
||||||
// Cutout starts half way through first preamble
|
// Cutout starts half way through first preamble
|
||||||
// that is 2.5 * 58uS later.
|
// that is 2.5 * 58uS later.
|
||||||
// TCNT1 ticks 8 times / microsecond
|
}
|
||||||
// auto microsendsToFirstRailcomTick=(58+58+29)-(TCNT1/8);
|
|
||||||
// set the railcom timer counter allowing for phase-correct
|
|
||||||
|
|
||||||
// CHris's NOTE:
|
|
||||||
// I dont kniow quite how this calculation works out but
|
|
||||||
// it does seems to get a good answer.
|
|
||||||
|
|
||||||
TCNT2=193 + (ICR1 - TCNT1)/8;
|
|
||||||
}
|
|
||||||
|
|
||||||
void DCCTimer::ackRailcomTimer() {
|
void DCCTimer::ackRailcomTimer() {
|
||||||
OCR2B= 0x00; // brake pin pwm duty cycle 0 at next tick
|
// Change Timer2 to CTC mode with RESET pin 9 on next compare match
|
||||||
|
TCCR2A = (1 << WGM21) | (1 << COM2B1);
|
||||||
|
// diagnostic digitalWrite(4,LOW);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -31,7 +31,7 @@
|
||||||
#include "DCCACK.h"
|
#include "DCCACK.h"
|
||||||
#include "DIAG.h"
|
#include "DIAG.h"
|
||||||
|
|
||||||
|
bool DCCWaveform::cutoutNextTime=false;
|
||||||
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);
|
||||||
|
|
||||||
|
@ -71,9 +71,14 @@ void DCCWaveform::loop() {
|
||||||
|
|
||||||
#pragma GCC push_options
|
#pragma GCC push_options
|
||||||
#pragma GCC optimize ("-O3")
|
#pragma GCC optimize ("-O3")
|
||||||
|
|
||||||
void DCCWaveform::interruptHandler() {
|
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
|
||||||
|
if (cutoutNextTime) {
|
||||||
|
cutoutNextTime=false;
|
||||||
|
DCCTimer::startRailcomTimer(9);
|
||||||
|
}
|
||||||
byte sigMain=signalTransform[mainTrack.state];
|
byte sigMain=signalTransform[mainTrack.state];
|
||||||
byte sigProg=TrackManager::progTrackSyncMain? sigMain : signalTransform[progTrack.state];
|
byte sigProg=TrackManager::progTrackSyncMain? sigMain : signalTransform[progTrack.state];
|
||||||
|
|
||||||
|
@ -140,6 +145,12 @@ void DCCWaveform::interrupt2() {
|
||||||
// or WAVE_HIGH_0 for a 0 bit.
|
// or WAVE_HIGH_0 for a 0 bit.
|
||||||
if (remainingPreambles > 0 ) {
|
if (remainingPreambles > 0 ) {
|
||||||
state=WAVE_MID_1; // switch state to trigger LOW on next interrupt
|
state=WAVE_MID_1; // switch state to trigger LOW on next interrupt
|
||||||
|
|
||||||
|
// predict railcom cutout on next interrupt
|
||||||
|
cutoutNextTime= remainingPreambles==requiredPreambles
|
||||||
|
&& railcomActive
|
||||||
|
&& isMainTrack;
|
||||||
|
|
||||||
remainingPreambles--;
|
remainingPreambles--;
|
||||||
|
|
||||||
// As we get to the end of the preambles, open the reminder window.
|
// As we get to the end of the preambles, open the reminder window.
|
||||||
|
@ -147,7 +158,7 @@ void DCCWaveform::interrupt2() {
|
||||||
// that the reminder doesn't block a more urgent packet.
|
// that the reminder doesn't block a more urgent packet.
|
||||||
reminderWindowOpen=transmitRepeats==0 && remainingPreambles<4 && remainingPreambles>1;
|
reminderWindowOpen=transmitRepeats==0 && remainingPreambles<4 && remainingPreambles>1;
|
||||||
if (remainingPreambles==1) promotePendingPacket();
|
if (remainingPreambles==1) promotePendingPacket();
|
||||||
else if (remainingPreambles==10 && isMainTrack && railcomActive) DCCTimer::ackRailcomTimer();
|
else if (remainingPreambles==14 && isMainTrack && railcomActive) DCCTimer::ackRailcomTimer();
|
||||||
// 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.
|
||||||
else DCCTimer::updateMinimumFreeMemoryISR(22);
|
else DCCTimer::updateMinimumFreeMemoryISR(22);
|
||||||
|
@ -171,13 +182,7 @@ void DCCWaveform::interrupt2() {
|
||||||
bytes_sent = 0;
|
bytes_sent = 0;
|
||||||
// preamble for next packet will start...
|
// preamble for next packet will start...
|
||||||
remainingPreambles = requiredPreambles;
|
remainingPreambles = requiredPreambles;
|
||||||
|
}
|
||||||
// set the railcom coundown to trigger half way
|
|
||||||
// through the first preamble bit.
|
|
||||||
// Note.. we are still sending the last packet bit
|
|
||||||
// and we then have to allow for the packet end bit
|
|
||||||
if (isMainTrack && railcomActive) DCCTimer::startRailcomTimer(9);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#pragma GCC pop_options
|
#pragma GCC pop_options
|
||||||
|
|
|
@ -114,7 +114,7 @@ class DCCWaveform {
|
||||||
byte pendingRepeats;
|
byte pendingRepeats;
|
||||||
static volatile bool railcomActive; // switched on by user
|
static volatile bool railcomActive; // switched on by user
|
||||||
static volatile bool railcomDebug; // switched on by user
|
static volatile bool railcomDebug; // switched on by user
|
||||||
|
static bool cutoutNextTime; // railcom
|
||||||
#ifdef ARDUINO_ARCH_ESP32
|
#ifdef ARDUINO_ARCH_ESP32
|
||||||
static RMTChannel *rmtMainChannel;
|
static RMTChannel *rmtMainChannel;
|
||||||
static RMTChannel *rmtProgChannel;
|
static RMTChannel *rmtProgChannel;
|
||||||
|
|
Loading…
Reference in New Issue
Block a user