diff --git a/DCC.cpp b/DCC.cpp index 0c5148a..bd07b84 100644 --- a/DCC.cpp +++ b/DCC.cpp @@ -138,7 +138,7 @@ void DCC::setFunctionInternal(int cab, byte byte1, byte byte2, byte count) { // returns speed steps 0 to 127 (1 == emergency stop) // or -1 on "loco not found" -int8_t DCC::getThrottleSpeed(int cab) { +static int8_t DCC::getThrottleSpeed(int cab) { int reg=lookupSpeedTable(cab); if (reg<0) return -1; return speedTable[reg].speedCode & 0x7F; @@ -146,13 +146,21 @@ int8_t DCC::getThrottleSpeed(int cab) { // returns speed code byte // or 128 (speed 0, dir forward) on "loco not found". -uint8_t DCC::getThrottleSpeedByte(int cab) { +static uint8_t DCC::getThrottleSpeedByte(int cab) { int reg=lookupSpeedTable(cab); if (reg<0) return 128; return speedTable[reg].speedCode; } +// returns -1 for fault, 0 to 3 for frequency +static int8_t DCC::getThrottleFrequency(int cab) { + int reg=lookupSpeedTable(cab); + if (reg<0) + return -1; + return (int8_t)(speedTable[reg].functions >>29); // shift out first 29 bits so we have the "frequency bits" left +} + // returns direction on loco // or true/forward on "loco not found" bool DCC::getThrottleDirection(int cab) { diff --git a/DCC.h b/DCC.h index 3bf0cf5..cbc5372 100644 --- a/DCC.h +++ b/DCC.h @@ -61,6 +61,7 @@ public: static void setThrottle(uint16_t cab, uint8_t tSpeed, bool tDirection); static int8_t getThrottleSpeed(int cab); static uint8_t getThrottleSpeedByte(int cab); + static int8_t getThrottleFrequency(int cab); static bool getThrottleDirection(int cab); static void writeCVByteMain(int cab, int cv, byte bValue); static void writeCVBitMain(int cab, int cv, byte bNum, bool bValue); diff --git a/MotorDriver.cpp b/MotorDriver.cpp index cdbd667..c1c9492 100644 --- a/MotorDriver.cpp +++ b/MotorDriver.cpp @@ -325,7 +325,7 @@ uint16_t taurustones[28] = { 165, 175, 196, 220, 220, 196, 175, 165 }; #endif #endif -void MotorDriver::setDCSignal(byte speedcode) { +void MotorDriver::setDCSignal(byte speedcode, uint8_t frequency /*default =0*/) { if (brakePin == UNUSED_PIN) return; // spedcoode is a dcc speed & direction @@ -341,7 +341,7 @@ void MotorDriver::setDCSignal(byte speedcode) { { // new block because of variable f #if defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_STM32) - int f = 131; + int f = frequency; #ifdef VARIABLE_TONES if (tSpeed > 2) { if (tSpeed <= 58) { @@ -352,7 +352,7 @@ void MotorDriver::setDCSignal(byte speedcode) { DCCTimer::DCCEXanalogWriteFrequency(brakePin, f); // set DC PWM frequency to 100Hz XXX May move to setup DCCTimer::DCCEXanalogWrite(brakePin,brake); #else // all AVR here - DCCTimer::DCCEXanalogWriteFrequency(brakePin, 0); // 0 is lowest possible f, like 120Hz + DCCTimer::DCCEXanalogWriteFrequency(brakePin, frequency); // frequency steps 0 to 3 analogWrite(brakePin,brake); #endif } diff --git a/MotorDriver.h b/MotorDriver.h index 07ff93f..b678a84 100644 --- a/MotorDriver.h +++ b/MotorDriver.h @@ -187,7 +187,7 @@ class MotorDriver { } }; inline pinpair getSignalPin() { return pinpair(signalPin,signalPin2); }; - void setDCSignal(byte speedByte); + void setDCSignal(byte speedByte, uint8_t frequency=0); void throttleInrush(bool on); inline void detachDCSignal() { #if defined(__arm__) diff --git a/TrackManager.cpp b/TrackManager.cpp index 4a501a1..f7be01a 100644 --- a/TrackManager.cpp +++ b/TrackManager.cpp @@ -348,8 +348,11 @@ bool TrackManager::setTrackMode(byte trackToSet, TRACK_MODE mode, int16_t dcAddr } void TrackManager::applyDCSpeed(byte t) { - uint8_t speedByte=DCC::getThrottleSpeedByte(trackDCAddr[t]); - track[t]->setDCSignal(speedByte); + int8_t frequency = DCC::getThrottleFrequency(trackDCAddr[t]); + if (frequency <0) // loco was not found + frequency = 0; // default + uint8_t speedByte = DCC::getThrottleSpeedByte(trackDCAddr[t]); + track[t]->setDCSignal(speedByte, (uint8_t)frequency); } bool TrackManager::parseEqualSign(Print *stream, int16_t params, int16_t p[])