Fix bug in mux function

MCUdude · Mar 15, 2024 · df18caf · df18caf
1 parent 33ed92f
commit df18caf
Showing 1 changed file with 12 additions and 1 deletion.
diff --git a/src/SigmaDSP.cpp b/src/SigmaDSP.cpp
@@ -90,7 +90,18 @@ void SigmaDSP::mux(uint16_t startMemoryAddress, uint8_t index, uint8_t numberOfI
   if(numberOfIndexes == 0)
     safeload_write(startMemoryAddress, index);
   else
-    demux(startMemoryAddress, index, numberOfIndexes);
+  {
+    uint8_t i = 0;
+    // Load leading zeros
+    for(; i < index; i++)
+      safeload_writeRegister(startMemoryAddress++, 0x00, false);
+    // Load index
+    i++;
+    safeload_writeRegister(startMemoryAddress++, 0x00800000, (i == numberOfIndexes ? true : false));
+    // Load tailing zeros
+    for(; i < numberOfIndexes; i++)
+      safeload_writeRegister(startMemoryAddress++, 0x00, (i == numberOfIndexes-1 ? true : false));
+  }
 }