From 3ce9d2ec8876953a000e15fe18756010ecb6bfd8 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Mon, 1 Jan 2024 22:08:04 +0100
Subject: [PATCH] DC frequency fix broadcast messages step #7

---
 CommandDistributor.cpp |  4 +++-
 DCC.cpp                | 20 +++++++++++++-------
 2 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/CommandDistributor.cpp b/CommandDistributor.cpp
index e8404de..f838fd2 100644
--- a/CommandDistributor.cpp
+++ b/CommandDistributor.cpp
@@ -209,7 +209,9 @@ int16_t CommandDistributor::retClockTime() {
 
 void  CommandDistributor::broadcastLoco(byte slot) {
   DCC::LOCO * sp=&DCC::speedTable[slot];
-  broadcastReply(COMMAND_TYPE, F("<l %d %d %d %l>\n"), sp->loco,slot,sp->speedCode,sp->functions);
+  uint32_t func = 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
   if (Serial2 && sp->loco == SABERTOOTH) {
     static uint8_t rampingmode = 0;
diff --git a/DCC.cpp b/DCC.cpp
index 59e08c9..76e1509 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -216,28 +216,34 @@ bool DCC::setFn( int cab, int16_t functionNumber, bool on) {
   } else {
       speedTable[reg].functions &= ~funcmask;
   }
-  if (speedTable[reg].functions != previous && functionNumber > 28) {
+  if (speedTable[reg].functions != previous && functionNumber <= 28) {
     updateGroupflags(speedTable[reg].groupFlags, functionNumber);
     CommandDistributor::broadcastLoco(reg);
   }
   return true;
 }
 
-// Flip function state
+// Flip function state (used from withrottle protocol)
 void DCC::changeFn( int cab, int16_t functionNumber) {
-  if (cab<=0 || functionNumber>28) return;
+  if (cab<=0 || functionNumber>31) return;
   int reg = lookupSpeedTable(cab);
   if (reg<0) return;
   unsigned long funcmask = (1UL<<functionNumber);
   speedTable[reg].functions ^= funcmask;
-  updateGroupflags(speedTable[reg].groupFlags, functionNumber);
-  CommandDistributor::broadcastLoco(reg);
+  if (functionNumber <= 28) {
+    updateGroupflags(speedTable[reg].groupFlags, functionNumber);
+    CommandDistributor::broadcastLoco(reg);
+  }
 }
 
+// Report function state (used from withrottle protocol)
+// returns 0 false, 1 true or -1 for do not know
 int DCC::getFn( int cab, int16_t functionNumber) {
-  if (cab<=0 || functionNumber>28) return -1;  // unknown
+  if (cab<=0 || functionNumber>28)
+    return -1;  // unknown
   int reg = lookupSpeedTable(cab);
-  if (reg<0) return -1;
+  if (reg<0)
+    return -1;
 
   unsigned long funcmask = (1UL<<functionNumber);
   return  (speedTable[reg].functions & funcmask)? 1 : 0;