1
0
mirror of https://github.com/DCC-EX/CommandStation-EX.git synced 2024-11-23 16:16:13 +01:00

Compare commits

..

No commits in common. "3fa2edb0dac4cbae560af447cc0982e481f6d67c" and "821115caaddd513152d277a5e5e4426bad8f9b58" have entirely different histories.

22 changed files with 128 additions and 270 deletions

View File

@ -209,9 +209,7 @@ int16_t CommandDistributor::retClockTime() {
void CommandDistributor::broadcastLoco(byte slot) { void CommandDistributor::broadcastLoco(byte slot) {
DCC::LOCO * sp=&DCC::speedTable[slot]; DCC::LOCO * sp=&DCC::speedTable[slot];
uint32_t func = sp->functions; broadcastReply(COMMAND_TYPE, F("<l %d %d %d %l>\n"), sp->loco,slot,sp->speedCode,sp->functions);
func = func & 0x1fffffff; // mask out bits 0-28
broadcastReply(COMMAND_TYPE, F("<l %d %d %d %l>\n"), sp->loco,slot,sp->speedCode,func);
#ifdef SABERTOOTH #ifdef SABERTOOTH
if (Serial2 && sp->loco == SABERTOOTH) { if (Serial2 && sp->loco == SABERTOOTH) {
static uint8_t rampingmode = 0; static uint8_t rampingmode = 0;

51
DCC.cpp
View File

@ -153,22 +153,6 @@ uint8_t DCC::getThrottleSpeedByte(int cab) {
return speedTable[reg].speedCode; return speedTable[reg].speedCode;
} }
// returns 0 to 7 for frequency
uint8_t DCC::getThrottleFrequency(int cab) {
#if defined(ARDUINO_AVR_UNO)
(void)cab;
return 0;
#else
int reg=lookupSpeedTable(cab);
if (reg<0)
return 0; // use default frequency
// shift out first 29 bits so we have the 3 "frequency bits" left
uint8_t res = (uint8_t)(speedTable[reg].functions >>29);
//DIAG(F("Speed table %d functions %l shifted %d"), reg, speedTable[reg].functions, res);
return res;
#endif
}
// returns direction on loco // returns direction on loco
// or true/forward on "loco not found" // or true/forward on "loco not found"
bool DCC::getThrottleDirection(int cab) { bool DCC::getThrottleDirection(int cab) {
@ -199,54 +183,43 @@ bool DCC::setFn( int cab, int16_t functionNumber, bool on) {
b[nB++] = functionNumber >>7 ; // high order bits b[nB++] = functionNumber >>7 ; // high order bits
} }
DCCWaveform::mainTrack.schedulePacket(b, nB, 4); DCCWaveform::mainTrack.schedulePacket(b, nB, 4);
}
// We use the reminder table up to 28 for normal functions.
// We use 29 to 31 for DC frequency as well so up to 28
// are "real" functions and 29 to 31 are frequency bits
// controlled by function buttons
if (functionNumber > 31)
return true; return true;
}
int reg = lookupSpeedTable(cab); int reg = lookupSpeedTable(cab);
if (reg<0) return false; if (reg<0) return false;
// Take care of functions: // Take care of functions:
// Set state of function // Set state of function
uint32_t previous=speedTable[reg].functions; unsigned long previous=speedTable[reg].functions;
uint32_t funcmask = (1UL<<functionNumber); unsigned long funcmask = (1UL<<functionNumber);
if (on) { if (on) {
speedTable[reg].functions |= funcmask; speedTable[reg].functions |= funcmask;
} else { } else {
speedTable[reg].functions &= ~funcmask; speedTable[reg].functions &= ~funcmask;
} }
if (speedTable[reg].functions != previous && functionNumber <= 28) { if (speedTable[reg].functions != previous) {
updateGroupflags(speedTable[reg].groupFlags, functionNumber); updateGroupflags(speedTable[reg].groupFlags, functionNumber);
CommandDistributor::broadcastLoco(reg); CommandDistributor::broadcastLoco(reg);
} }
return true; return true;
} }
// Flip function state (used from withrottle protocol) // Flip function state
void DCC::changeFn( int cab, int16_t functionNumber) { void DCC::changeFn( int cab, int16_t functionNumber) {
if (cab<=0 || functionNumber>31) return; if (cab<=0 || functionNumber>28) return;
int reg = lookupSpeedTable(cab); int reg = lookupSpeedTable(cab);
if (reg<0) return; if (reg<0) return;
unsigned long funcmask = (1UL<<functionNumber); unsigned long funcmask = (1UL<<functionNumber);
speedTable[reg].functions ^= funcmask; speedTable[reg].functions ^= funcmask;
if (functionNumber <= 28) { updateGroupflags(speedTable[reg].groupFlags, functionNumber);
updateGroupflags(speedTable[reg].groupFlags, functionNumber); CommandDistributor::broadcastLoco(reg);
CommandDistributor::broadcastLoco(reg);
}
} }
// Report function state (used from withrottle protocol) int DCC::getFn( int cab, int16_t functionNumber) {
// returns 0 false, 1 true or -1 for do not know if (cab<=0 || functionNumber>28) return -1; // unknown
int8_t DCC::getFn( int cab, int16_t functionNumber) {
if (cab<=0 || functionNumber>28)
return -1; // unknown
int reg = lookupSpeedTable(cab); int reg = lookupSpeedTable(cab);
if (reg<0) if (reg<0) return -1;
return -1;
unsigned long funcmask = (1UL<<functionNumber); unsigned long funcmask = (1UL<<functionNumber);
return (speedTable[reg].functions & funcmask)? 1 : 0; return (speedTable[reg].functions & funcmask)? 1 : 0;

5
DCC.h
View File

@ -61,14 +61,13 @@ public:
static void setThrottle(uint16_t cab, uint8_t tSpeed, bool tDirection); static void setThrottle(uint16_t cab, uint8_t tSpeed, bool tDirection);
static int8_t getThrottleSpeed(int cab); static int8_t getThrottleSpeed(int cab);
static uint8_t getThrottleSpeedByte(int cab); static uint8_t getThrottleSpeedByte(int cab);
static uint8_t getThrottleFrequency(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);
static void setFunction(int cab, byte fByte, byte eByte); static void setFunction(int cab, byte fByte, byte eByte);
static bool setFn(int cab, int16_t functionNumber, bool on); static bool setFn(int cab, int16_t functionNumber, bool on);
static void changeFn(int cab, int16_t functionNumber); static void changeFn(int cab, int16_t functionNumber);
static int8_t getFn(int cab, int16_t functionNumber); static int getFn(int cab, int16_t functionNumber);
static uint32_t getFunctionMap(int cab); static 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 address, byte port, bool gate, byte onoff = 2); static void setAccessory(int address, byte port, bool gate, byte onoff = 2);
@ -100,7 +99,7 @@ public:
int loco; int loco;
byte speedCode; byte speedCode;
byte groupFlags; byte groupFlags;
uint32_t functions; unsigned long functions;
}; };
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);

View File

@ -582,13 +582,12 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
DCC::setThrottle(0,1,1); // this broadcasts speed 1(estop) and sets all reminders to speed 1. DCC::setThrottle(0,1,1); // this broadcasts speed 1(estop) and sets all reminders to speed 1.
return; return;
#ifdef HAS_ENOUGH_MEMORY
case 'c': // SEND METER RESPONSES <c> case 'c': // SEND METER RESPONSES <c>
// No longer useful because of multiple tracks See <JG> and <JI> // No longer useful because of multiple tracks See <JG> and <JI>
if (params>0) break; if (params>0) break;
TrackManager::reportObsoleteCurrent(stream); TrackManager::reportObsoleteCurrent(stream);
return; return;
#endif
case 'Q': // SENSORS <Q> case 'Q': // SENSORS <Q>
Sensor::printAll(stream); Sensor::printAll(stream);
return; return;

View File

@ -87,7 +87,6 @@ class DCCTimer {
static void reset(); static void reset();
private: private:
static void DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t frequency);
static int freeMemory(); static int freeMemory();
static volatile int minimum_free_memory; 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 int DCC_SIGNAL_TIME=58; // this is the 58uS DCC 1-bit waveform half-cycle

View File

@ -29,7 +29,6 @@
#include <avr/boot.h> #include <avr/boot.h>
#include <avr/wdt.h> #include <avr/wdt.h>
#include "DCCTimer.h" #include "DCCTimer.h"
#include "DIAG.h"
#ifdef DEBUG_ADC #ifdef DEBUG_ADC
#include "TrackManager.h" #include "TrackManager.h"
#endif #endif
@ -190,81 +189,6 @@ void DCCTimer::reset() {
} }
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, f);
}
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
#if defined(ARDUINO_AVR_UNO)
// Not worth doin something here as:
// If we are on pin 9 or 10 we are on Timer1 and we can not touch Timer1 as that is our DCC source.
// If we are on pin 5 or 6 we are on Timer 0 ad we can not touch Timer0 as that is millis() etc.
// We are most likely not on pin 3 or 11 as no known motor shield has that as brake.
#endif
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
// Speed mapping is done like this:
// No functions buttons: 000 0 -> low 131Hz
// Only F29 pressed 001 1 -> mid 490Hz
// F30 with or w/o F29 01x 2-3 -> high 3400Hz
// F31 with or w/o F29/30 1xx 4-7 -> supersonic 62500Hz
uint8_t abits;
uint8_t bbits;
if (pin == 9 || pin == 10) { // timer 2 is different
if (fbits >= 4)
abits = B00000011;
else
abits = B00000001;
if (fbits >= 4)
bbits = B0001;
else if (fbits >= 2)
bbits = B0010;
else if (fbits == 1)
bbits = B0100;
else // fbits == 0
bbits = B0110;
TCCR2A = (TCCR2A & B11111100) | abits; // set WGM0 and WGM1
TCCR2B = (TCCR2B & B11110000) | bbits; // set WGM2 and 3 bits of prescaler
DIAG(F("Timer 2 A=%x B=%x"), TCCR2A, TCCR2B);
} else { // not timer 9 or 10
abits = B01;
if (fbits >= 4)
bbits = B1001;
else if (fbits >= 2)
bbits = B0010;
else if (fbits == 1)
bbits = B0011;
else
bbits = B0100;
switch (pin) {
// case 9 and 10 taken care of above by if()
case 6:
case 7:
case 8:
// Timer4
TCCR4A = (TCCR4A & B11111100) | abits; // set WGM0 and WGM1
TCCR4B = (TCCR4B & B11100000) | bbits; // set WGM2 and WGM3 and divisor
//DIAG(F("Timer 4 A=%x B=%x"), TCCR4A, TCCR4B);
break;
case 46:
case 45:
case 44:
// Timer5
TCCR5A = (TCCR5A & B11111100) | abits; // set WGM0 and WGM1
TCCR5B = (TCCR5B & B11100000) | bbits; // set WGM2 and WGM3 and divisor
//DIAG(F("Timer 5 A=%x B=%x"), TCCR5A, TCCR5B);
break;
default:
break;
}
}
#endif
}
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560) #if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
#define NUM_ADC_INPUTS 16 #define NUM_ADC_INPUTS 16
#else #else

View File

@ -151,26 +151,10 @@ void DCCTimer::reset() {
ESP.restart(); ESP.restart();
} }
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
if (f >= 16)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, f);
else if (f == 7)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 62500);
else if (f >= 4)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 32000);
else if (f >= 3)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 16000);
else if (f >= 2)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 3400);
else if (f == 1)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 480);
else
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 131);
}
#include "esp32-hal.h" #include "esp32-hal.h"
#include "soc/soc_caps.h" #include "soc/soc_caps.h"
#ifdef SOC_LEDC_SUPPORT_HS_MODE #ifdef SOC_LEDC_SUPPORT_HS_MODE
#define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM<<1) #define LEDC_CHANNELS (SOC_LEDC_CHANNEL_NUM<<1)
#else #else
@ -180,7 +164,7 @@ void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
static int8_t pin_to_channel[SOC_GPIO_PIN_COUNT] = { 0 }; static int8_t pin_to_channel[SOC_GPIO_PIN_COUNT] = { 0 };
static int cnt_channel = LEDC_CHANNELS; static int cnt_channel = LEDC_CHANNELS;
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t frequency) { void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t frequency) {
if (pin < SOC_GPIO_PIN_COUNT) { if (pin < SOC_GPIO_PIN_COUNT) {
if (pin_to_channel[pin] != 0) { if (pin_to_channel[pin] != 0) {
ledcSetup(pin_to_channel[pin], frequency, 8); ledcSetup(pin_to_channel[pin], frequency, 8);

View File

@ -134,11 +134,6 @@ void DCCTimer::reset() {
while(true){} while(true){}
} }
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
}
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
}
int16_t ADCee::ADCmax() { int16_t ADCee::ADCmax() {
return 4095; return 4095;
} }

View File

@ -165,11 +165,6 @@ void DCCTimer::reset() {
while(true) {}; while(true) {};
} }
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
}
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
}
#define NUM_ADC_INPUTS NUM_ANALOG_INPUTS #define NUM_ADC_INPUTS NUM_ANALOG_INPUTS
uint16_t ADCee::usedpins = 0; uint16_t ADCee::usedpins = 0;

View File

@ -266,23 +266,6 @@ void DCCTimer::reset() {
while(true) {}; while(true) {};
} }
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
if (f >= 16)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, f);
else if (f == 7)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 62500);
else if (f >= 4)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 32000);
else if (f >= 3)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 16000);
else if (f >= 2)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 3400);
else if (f == 1)
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 480);
else
DCCTimer::DCCEXanalogWriteFrequencyInternal(pin, 131);
}
// TODO: rationalise the size of these... could really use sparse arrays etc. // TODO: rationalise the size of these... could really use sparse arrays etc.
static HardwareTimer * pin_timer[100] = {0}; static HardwareTimer * pin_timer[100] = {0};
static uint32_t channel_frequency[100] = {0}; static uint32_t channel_frequency[100] = {0};
@ -293,7 +276,7 @@ static uint32_t pin_channel[100] = {0};
// sophisticated about detecting any clash between the timer we'd like to use for PWM and the ones // sophisticated about detecting any clash between the timer we'd like to use for PWM and the ones
// currently used for HA so they don't interfere with one another. For now we'll just make PWM // currently used for HA so they don't interfere with one another. For now we'll just make PWM
// work well... then work backwards to integrate with HA mode if we can. // work well... then work backwards to integrate with HA mode if we can.
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t frequency) void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t frequency)
{ {
if (pin_timer[pin] == NULL) { if (pin_timer[pin] == NULL) {
// Automatically retrieve TIM instance and channel associated to pin // Automatically retrieve TIM instance and channel associated to pin

View File

@ -150,11 +150,6 @@ void DCCTimer::reset() {
SCB_AIRCR = 0x05FA0004; SCB_AIRCR = 0x05FA0004;
} }
void DCCTimer::DCCEXanalogWriteFrequency(uint8_t pin, uint32_t f) {
}
void DCCTimer::DCCEXanalogWriteFrequencyInternal(uint8_t pin, uint32_t fbits) {
}
int16_t ADCee::ADCmax() { int16_t ADCee::ADCmax() {
return 4095; return 4095;
} }

View File

@ -669,45 +669,6 @@ void RMFT2::loop2() {
} }
break; break;
case OPCODE_SETFREQ:
// Frequency is default 0, or 1, 2,3
//if (loco) DCC::setFn(loco,operand,true);
switch (operand) {
case 0: // default - all F-s off
if (loco) {
DCC::setFn(loco,29,false);
DCC::setFn(loco,30,false);
DCC::setFn(loco,31,false);
}
break;
case 1:
if (loco) {
DCC::setFn(loco,29,true);
DCC::setFn(loco,30,false);
DCC::setFn(loco,31,false);
}
break;
case 2:
if (loco) {
DCC::setFn(loco,29,false);
DCC::setFn(loco,30,true);
DCC::setFn(loco,31,false);
}
break;
case 3:
if (loco) {
DCC::setFn(loco,29,false);
DCC::setFn(loco,30,false);
DCC::setFn(loco,31,true);
}
break;
default:
; // do nothing
break;
}
break;
case OPCODE_RESUME: case OPCODE_RESUME:
pausingTask=NULL; pausingTask=NULL;
driveLoco(speedo); driveLoco(speedo);

View File

@ -51,7 +51,7 @@ enum OPCODE : byte {OPCODE_THROW,OPCODE_CLOSE,
OPCODE_JOIN,OPCODE_UNJOIN,OPCODE_READ_LOCO1,OPCODE_READ_LOCO2, OPCODE_JOIN,OPCODE_UNJOIN,OPCODE_READ_LOCO1,OPCODE_READ_LOCO2,
#endif #endif
OPCODE_POM, OPCODE_POM,
OPCODE_START,OPCODE_SETLOCO,OPCODE_SETFREQ,OPCODE_SENDLOCO,OPCODE_FORGET, OPCODE_START,OPCODE_SETLOCO,OPCODE_SENDLOCO,OPCODE_FORGET,
OPCODE_PAUSE, OPCODE_RESUME,OPCODE_POWEROFF,OPCODE_POWERON, OPCODE_PAUSE, OPCODE_RESUME,OPCODE_POWEROFF,OPCODE_POWERON,
OPCODE_ONCLOSE, OPCODE_ONTHROW, OPCODE_SERVOTURNOUT, OPCODE_PINTURNOUT, OPCODE_ONCLOSE, OPCODE_ONTHROW, OPCODE_SERVOTURNOUT, OPCODE_PINTURNOUT,
OPCODE_PRINT,OPCODE_DCCACTIVATE,OPCODE_ASPECT, OPCODE_PRINT,OPCODE_DCCACTIVATE,OPCODE_ASPECT,

View File

@ -156,7 +156,6 @@
#undef SET_TRACK #undef SET_TRACK
#undef SET_POWER #undef SET_POWER
#undef SETLOCO #undef SETLOCO
#undef SETFREQ
#undef SIGNAL #undef SIGNAL
#undef SIGNALH #undef SIGNALH
#undef SPEED #undef SPEED
@ -313,7 +312,6 @@
#define SET_TRACK(track,mode) #define SET_TRACK(track,mode)
#define SET_POWER(track,onoff) #define SET_POWER(track,onoff)
#define SETLOCO(loco) #define SETLOCO(loco)
#define SETFREQ(loco,freq)
#define SIGNAL(redpin,amberpin,greenpin) #define SIGNAL(redpin,amberpin,greenpin)
#define SIGNALH(redpin,amberpin,greenpin) #define SIGNALH(redpin,amberpin,greenpin)
#define SPEED(speed) #define SPEED(speed)

View File

@ -581,7 +581,6 @@ int RMFT2::onLCCLookup[RMFT2::countLCCLookup];
#define SET_TRACK(track,mode) OPCODE_SET_TRACK,V(TRACK_MODE_##mode <<8 | TRACK_NUMBER_##track), #define SET_TRACK(track,mode) OPCODE_SET_TRACK,V(TRACK_MODE_##mode <<8 | TRACK_NUMBER_##track),
#define SET_POWER(track,onoff) OPCODE_SET_POWER,V(TRACK_POWER_##onoff),OPCODE_PAD, V(TRACK_NUMBER_##track), #define SET_POWER(track,onoff) OPCODE_SET_POWER,V(TRACK_POWER_##onoff),OPCODE_PAD, V(TRACK_NUMBER_##track),
#define SETLOCO(loco) OPCODE_SETLOCO,V(loco), #define SETLOCO(loco) OPCODE_SETLOCO,V(loco),
#define SETFREQ(loco,freq) OPCODE_SETLOCO,V(loco), OPCODE_SETFREQ,V(freq),
#define SIGNAL(redpin,amberpin,greenpin) #define SIGNAL(redpin,amberpin,greenpin)
#define SIGNALH(redpin,amberpin,greenpin) #define SIGNALH(redpin,amberpin,greenpin)
#define SPEED(speed) OPCODE_SPEED,V(speed), #define SPEED(speed) OPCODE_SPEED,V(speed),

View File

@ -1 +1 @@
#define GITHUB_SHA "devel-202402201404Z" #define GITHUB_SHA "devel-202402171752Z"

View File

@ -325,23 +325,49 @@ uint16_t taurustones[28] = { 165, 175, 196, 220,
220, 196, 175, 165 }; 220, 196, 175, 165 };
#endif #endif
#endif #endif
void MotorDriver::setDCSignal(byte speedcode, uint8_t frequency /*default =0*/) { void MotorDriver::setDCSignal(byte speedcode) {
if (brakePin == UNUSED_PIN) if (brakePin == UNUSED_PIN)
return; return;
switch(brakePin) {
#if defined(ARDUINO_AVR_UNO)
// Not worth doin something here as:
// If we are on pin 9 or 10 we are on Timer1 and we can not touch Timer1 as that is our DCC source.
// If we are on pin 5 or 6 we are on Timer 0 ad we can not touch Timer0 as that is millis() etc.
// We are most likely not on pin 3 or 11 as no known motor shield has that as brake.
#endif
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
case 9:
case 10:
// Timer2 (is differnet)
TCCR2A = (TCCR2A & B11111100) | B00000001; // set WGM1=0 and WGM0=1 phase correct PWM
TCCR2B = (TCCR2B & B11110000) | B00000110; // set WGM2=0 ; set divisor on timer 2 to 1/256 for 122.55Hz
//DIAG(F("2 A=%x B=%x"), TCCR2A, TCCR2B);
break;
case 6:
case 7:
case 8:
// Timer4
TCCR4A = (TCCR4A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for normal PWM 8-bit
TCCR4B = (TCCR4B & B11100000) | B00000100; // set WGM2=0 and WGM3=0 for normal PWM 8 bit and div 1/256 for 122.55Hz
break;
case 46:
case 45:
case 44:
// Timer5
TCCR5A = (TCCR5A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for normal PWM 8-bit
TCCR5B = (TCCR5B & B11100000) | B00000100; // set WGM2=0 and WGM3=0 for normal PWM 8 bit and div 1/256 for 122.55Hz
break;
#endif
default:
break;
}
// spedcoode is a dcc speed & direction // spedcoode is a dcc speed & direction
byte tSpeed=speedcode & 0x7F; // DCC Speed with 0,1 stop and speed steps 2 to 127 byte tSpeed=speedcode & 0x7F; // DCC Speed with 0,1 stop and speed steps 2 to 127
byte tDir=speedcode & 0x80; byte tDir=speedcode & 0x80;
byte brake; byte brake;
if (tSpeed <= 1) brake = 255;
else if (tSpeed >= 127) brake = 0;
else brake = 2 * (128-tSpeed);
if (invertBrake)
brake=255-brake;
{ // new block because of variable f
#if defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_STM32) #if defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_STM32)
int f = frequency; {
int f = 131;
#ifdef VARIABLE_TONES #ifdef VARIABLE_TONES
if (tSpeed > 2) { if (tSpeed > 2) {
if (tSpeed <= 58) { if (tSpeed <= 58) {
@ -349,15 +375,19 @@ void MotorDriver::setDCSignal(byte speedcode, uint8_t frequency /*default =0*/)
} }
} }
#endif #endif
//DIAG(F("Brake pin %d freqency %d"), brakePin, f); DCCTimer::DCCEXanalogWriteFrequency(brakePin, f); // set DC PWM frequency to 100Hz XXX May move to setup
DCCTimer::DCCEXanalogWriteFrequency(brakePin, f); // set DC PWM frequency
DCCTimer::DCCEXanalogWrite(brakePin,brake);
#else // all AVR here
DCCTimer::DCCEXanalogWriteFrequency(brakePin, frequency); // frequency steps
analogWrite(brakePin,brake);
#endif
} }
#endif
if (tSpeed <= 1) brake = 255;
else if (tSpeed >= 127) brake = 0;
else brake = 2 * (128-tSpeed);
if (invertBrake)
brake=255-brake;
#if defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_STM32)
DCCTimer::DCCEXanalogWrite(brakePin,brake);
#else
analogWrite(brakePin,brake);
#endif
//DIAG(F("DCSignal %d"), speedcode); //DIAG(F("DCSignal %d"), speedcode);
if (HAVE_PORTA(fastSignalPin.shadowinout == &PORTA)) { if (HAVE_PORTA(fastSignalPin.shadowinout == &PORTA)) {
noInterrupts(); noInterrupts();
@ -406,26 +436,58 @@ void MotorDriver::throttleInrush(bool on) {
return; return;
if ( !(trackMode & (TRACK_MODE_MAIN | TRACK_MODE_PROG | TRACK_MODE_EXT))) if ( !(trackMode & (TRACK_MODE_MAIN | TRACK_MODE_PROG | TRACK_MODE_EXT)))
return; return;
byte duty = on ? 207 : 0; // duty of 81% at 62500Hz this gives pauses of 3usec byte duty = on ? 208 : 0;
if (invertBrake) if (invertBrake)
duty = 255-duty; duty = 255-duty;
#if defined(ARDUINO_ARCH_ESP32) #if defined(ARDUINO_ARCH_ESP32)
if(on) { if(on) {
DCCTimer::DCCEXanalogWrite(brakePin,duty); DCCTimer::DCCEXanalogWrite(brakePin,duty);
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 7); // 7 means max DCCTimer::DCCEXanalogWriteFrequency(brakePin, 62500);
} else { } else {
ledcDetachPin(brakePin); ledcDetachPin(brakePin);
} }
#elif defined(ARDUINO_ARCH_STM32) #elif defined(ARDUINO_ARCH_STM32)
if(on) { if(on) {
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 7); // 7 means max DCCTimer::DCCEXanalogWriteFrequency(brakePin, 62500);
DCCTimer::DCCEXanalogWrite(brakePin,duty); DCCTimer::DCCEXanalogWrite(brakePin,duty);
} else { } else {
pinMode(brakePin, OUTPUT); pinMode(brakePin, OUTPUT);
} }
#else // all AVR here #else
if(on){ if(on){
DCCTimer::DCCEXanalogWriteFrequency(brakePin, 7); // 7 means max switch(brakePin) {
#if defined(ARDUINO_AVR_UNO)
// Not worth doin something here as:
// If we are on pin 9 or 10 we are on Timer1 and we can not touch Timer1 as that is our DCC source.
// If we are on pin 5 or 6 we are on Timer 0 ad we can not touch Timer0 as that is millis() etc.
// We are most likely not on pin 3 or 11 as no known motor shield has that as brake.
#endif
#if defined(ARDUINO_AVR_MEGA) || defined(ARDUINO_AVR_MEGA2560)
case 9:
case 10:
// Timer2 (is different)
TCCR2A = (TCCR2A & B11111100) | B00000011; // set WGM0=1 and WGM1=1 for fast PWM
TCCR2B = (TCCR2B & B11110000) | B00000001; // set WGM2=0 and prescaler div=1 (max)
DIAG(F("2 A=%x B=%x"), TCCR2A, TCCR2B);
break;
case 6:
case 7:
case 8:
// Timer4
TCCR4A = (TCCR4A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for fast PWM 8-bit
TCCR4B = (TCCR4B & B11100000) | B00001001; // set WGM2=1 and WGM3=0 for fast PWM 8 bit and div=1 (max)
break;
case 46:
case 45:
case 44:
// Timer5
TCCR5A = (TCCR5A & B11111100) | B00000001; // set WGM0=1 and WGM1=0 for fast PWM 8-bit
TCCR5B = (TCCR5B & B11100000) | B00001001; // set WGM2=1 and WGM3=0 for fast PWM 8 bit and div=1 (max)
break;
#endif
default:
break;
}
} }
analogWrite(brakePin,duty); analogWrite(brakePin,duty);
#endif #endif

View File

@ -187,7 +187,7 @@ class MotorDriver {
} }
}; };
inline pinpair getSignalPin() { return pinpair(signalPin,signalPin2); }; inline pinpair getSignalPin() { return pinpair(signalPin,signalPin2); };
void setDCSignal(byte speedByte, uint8_t frequency=0); void setDCSignal(byte speedByte);
void throttleInrush(bool on); void throttleInrush(bool on);
inline void detachDCSignal() { inline void detachDCSignal() {
#if defined(__arm__) #if defined(__arm__)

View File

@ -19,7 +19,6 @@
* 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/>.
*/ */
#include "defines.h"
#include "TrackManager.h" #include "TrackManager.h"
#include "FSH.h" #include "FSH.h"
#include "DCCWaveform.h" #include "DCCWaveform.h"
@ -183,7 +182,7 @@ void TrackManager::setDCSignal(int16_t cab, byte speedbyte) {
FOR_EACH_TRACK(t) { FOR_EACH_TRACK(t) {
if (trackDCAddr[t]!=cab && cab != 0) continue; if (trackDCAddr[t]!=cab && cab != 0) continue;
if (track[t]->getMode() & TRACK_MODE_DC) if (track[t]->getMode() & TRACK_MODE_DC)
track[t]->setDCSignal(speedbyte, DCC::getThrottleFrequency(trackDCAddr[t])); track[t]->setDCSignal(speedbyte);
} }
} }
@ -329,8 +328,8 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr
} }
void TrackManager::applyDCSpeed(byte t) { void TrackManager::applyDCSpeed(byte t) {
track[t]->setDCSignal(DCC::getThrottleSpeedByte(trackDCAddr[t]), uint8_t speedByte=DCC::getThrottleSpeedByte(trackDCAddr[t]);
DCC::getThrottleFrequency(trackDCAddr[t])); track[t]->setDCSignal(speedByte);
} }
bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[]) bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])
@ -555,17 +554,14 @@ bool TrackManager::getPower(byte t, char s[]) {
return false; return false;
} }
void TrackManager::reportObsoleteCurrent(Print* stream) { void TrackManager::reportObsoleteCurrent(Print* stream) {
// This function is for backward JMRI compatibility only // This function is for backward JMRI compatibility only
// It reports the first track only, as main, regardless of track settings. // It reports the first track only, as main, regardless of track settings.
// <c MeterName value C/V unit min max res warn> // <c MeterName value C/V unit min max res warn>
#ifdef HAS_ENOUGH_MEMORY
int maxCurrent=track[0]->raw2mA(track[0]->getRawCurrentTripValue()); int maxCurrent=track[0]->raw2mA(track[0]->getRawCurrentTripValue());
StringFormatter::send(stream, F("<c CurrentMAIN %d C Milli 0 %d 1 %d>\n"), StringFormatter::send(stream, F("<c CurrentMAIN %d C Milli 0 %d 1 %d>\n"),
track[0]->raw2mA(track[0]->getCurrentRaw(false)), maxCurrent, maxCurrent); track[0]->raw2mA(track[0]->getCurrentRaw(false)), maxCurrent, maxCurrent);
#else
(void)stream;
#endif
} }
void TrackManager::reportCurrent(Print* stream) { void TrackManager::reportCurrent(Print* stream) {

View File

@ -247,23 +247,22 @@ DCCTurntable::DCCTurntable(uint16_t id) : Turntable(id, TURNTABLE_DCC) {}
StringFormatter::send(stream, F("<i %d DCCTURNTABLE>\n"), _turntableData.id); StringFormatter::send(stream, F("<i %d DCCTURNTABLE>\n"), _turntableData.id);
} }
// EX-Turntable specific code for moving to the specified position // EX-Turntable specific code for moving to the specified position
bool DCCTurntable::setPositionInternal(uint8_t position, uint8_t activity) { bool DCCTurntable::setPositionInternal(uint8_t position, uint8_t activity) {
(void) activity;
#ifndef IO_NO_HAL #ifndef IO_NO_HAL
int16_t value = getPositionValue(position); int16_t value = getPositionValue(position);
if (position == 0 || !value) return false; // Return false if it's not a valid position if (position == 0 || !value) return false; // Return false if it's not a valid position
// Set position via device driver // Set position via device driver
int16_t addr=value>>3; int16_t addr=value>>3;
int16_t subaddr=(value>>1) & 0x03; int16_t subaddr=(value>>1) & 0x03;
bool active=value & 0x01; bool active=value & 0x01;
_previousPosition = _turntableData.position; _previousPosition = _turntableData.position;
_turntableData.position = position; _turntableData.position = position;
DCC::setAccessory(addr, subaddr, active); DCC::setAccessory(addr, subaddr, active);
#else #else
(void)position; (void)position;
#endif #endif
return true; return true;
} }
#endif #endif

View File

@ -621,7 +621,7 @@ void WiThrottle::sendFunctions(Print* stream, byte loco) {
#endif #endif
for(int fKey=0; fKey<fkeys; fKey++) { for(int fKey=0; fKey<fkeys; fKey++) {
int8_t fstate=DCC::getFn(locoid,fKey); int fstate=DCC::getFn(locoid,fKey);
if (fstate>=0) StringFormatter::send(stream,F("M%cA%c%d<;>F%d%d\n"),myLocos[loco].throttle,LorS(locoid),locoid,fstate,fKey); if (fstate>=0) StringFormatter::send(stream,F("M%cA%c%d<;>F%d%d\n"),myLocos[loco].throttle,LorS(locoid),locoid,fstate,fKey);
} }
} }

View File

@ -3,8 +3,7 @@
#include "StringFormatter.h" #include "StringFormatter.h"
#define VERSION "5.2.36" #define VERSION "5.2.35"
// 5.2.36 - Variable frequency for DC mode
// 5.2.35 - Bugfix: Make DCC Extended Accessories follow RCN-213 // 5.2.35 - Bugfix: Make DCC Extended Accessories follow RCN-213
// 5.2.34 - <A address aspect> Command fopr DCC Extended Accessories // 5.2.34 - <A address aspect> Command fopr DCC Extended Accessories
// - Exrail ASPECT(address,aspect) for above. // - Exrail ASPECT(address,aspect) for above.