VFD functionality successfully integrated into Gaugecontroller

Gaugecontroller/Gaugecontroller.ino

@@ -1,4 +1,5 @@
 #include <Arduino.h>
+#include <ctype.h>
 #include <math.h>
 #include <FastLED.h>
 
@@ -12,6 +13,212 @@ static const uint8_t LED_DATA_PIN = 22;
 #define DEBUG_PORT Serial1
 static const unsigned long SERIAL_BAUD = 38400;
 
+namespace vfd {
+
+constexpr uint8_t kDataPin = 46;
+constexpr uint8_t kClockPin = 47;
+constexpr uint8_t kLatchPin = 48;
+constexpr int8_t kBlankPin = 49;   // Set to -1 if BL/OE is not connected
+constexpr bool kBlankActiveHigh = true;
+
+constexpr unsigned long kDigitHoldMicros = 2000;
+constexpr uint8_t kDigitCount = 4;
+constexpr uint8_t kSegmentCount = 7;
+constexpr uint8_t kDriverBits = 20;
+
+constexpr uint8_t kSegmentStartBit = 0;    // HVOut1 -> bit 0
+constexpr uint8_t kPointSegmentBit = 7;    // HVOut8 -> bit 7
+constexpr uint8_t kBellSegmentBit = 8;     // HVOut9 -> bit 8
+constexpr uint8_t kGridStartBit = 9;       // HVOut10 -> bit 9
+constexpr uint8_t kIndicatorGridBit = 13;  // HVOut14 -> bit 13
+
+char displayBuffer[kDigitCount] = {' ', ' ', ' ', ' '};
+bool pointEnabled = false;
+bool bellEnabled = false;
+uint8_t currentPhase = 0;
+
+uint8_t encodeCharacter(char c) {
+  switch (c) {
+    case '0': return 0b0111111;
+    case '1': return 0b0000110;
+    case '2': return 0b1011011;
+    case '3': return 0b1001111;
+    case '4': return 0b1100110;
+    case '5': return 0b1101101;
+    case '6': return 0b1111101;
+    case '7': return 0b0000111;
+    case '8': return 0b1111111;
+    case '9': return 0b1101111;
+    case 'A':
+    case 'a': return 0b1110111;
+    case 'B':
+    case 'b': return 0b1111100;
+    case 'C':
+    case 'c': return 0b0111001;
+    case 'D':
+    case 'd': return 0b1011110;
+    case 'E':
+    case 'e': return 0b1111001;
+    case 'F':
+    case 'f': return 0b1110001;
+    case '-': return 0b1000000;
+    default: return 0;
+  }
+}
+
+void shiftDriverWord(uint32_t word) {
+  digitalWrite(kLatchPin, HIGH);
+  digitalWrite(kClockPin, HIGH);
+
+  for (int8_t bit = kDriverBits - 1; bit >= 0; --bit) {
+    digitalWrite(kDataPin, (word >> bit) & 0x1U ? HIGH : LOW);
+    digitalWrite(kClockPin, LOW);
+    digitalWrite(kClockPin, HIGH);
+  }
+
+  digitalWrite(kLatchPin, LOW);
+  digitalWrite(kLatchPin, HIGH);
+}
+
+void setBlanked(bool blanked) {
+  if (kBlankPin < 0) return;
+
+  const bool level = kBlankActiveHigh ? blanked : !blanked;
+  digitalWrite(kBlankPin, level ? HIGH : LOW);
+}
+
+void writeText(const char* text) {
+  for (uint8_t i = 0; i < kDigitCount; ++i) {
+    displayBuffer[i] = ' ';
+  }
+
+  size_t len = strlen(text);
+  if (len > kDigitCount) {
+    text += len - kDigitCount;
+    len = kDigitCount;
+  }
+
+  const uint8_t start = kDigitCount - len;
+  for (uint8_t i = 0; i < len; ++i) {
+    displayBuffer[start + i] = text[i];
+  }
+}
+
+void clear() {
+  writeText("");
+  pointEnabled = false;
+  bellEnabled = false;
+}
+
+bool parseCommand(const String& command) {
+  char displayText[16];
+  size_t inputIndex = 0;
+  size_t displayIndex = 0;
+
+  if (command.length() == 0) {
+    return false;
+  }
+
+  if (command[inputIndex] == '-') {
+    if (displayIndex + 1 >= sizeof(displayText)) {
+      return false;
+    }
+    displayText[displayIndex++] = command[inputIndex++];
+  }
+
+  const size_t digitStart = inputIndex;
+  while (inputIndex < static_cast<size_t>(command.length()) &&
+         isxdigit(static_cast<unsigned char>(command[inputIndex]))) {
+    if (displayIndex + 1 >= sizeof(displayText)) {
+      return false;
+    }
+    displayText[displayIndex++] = toupper(static_cast<unsigned char>(command[inputIndex]));
+    ++inputIndex;
+  }
+
+  if (inputIndex == digitStart) {
+    return false;
+  }
+
+  bool newPointEnabled = false;
+  bool newBellEnabled = false;
+  while (inputIndex < static_cast<size_t>(command.length())) {
+    if (command[inputIndex] == '.') {
+      newPointEnabled = true;
+    } else if (command[inputIndex] == '!') {
+      newBellEnabled = true;
+    } else {
+      return false;
+    }
+    ++inputIndex;
+  }
+
+  displayText[displayIndex] = '\0';
+  writeText(displayText);
+  pointEnabled = newPointEnabled;
+  bellEnabled = newBellEnabled;
+  return true;
+}
+
+void renderDigit(uint8_t digitIndex) {
+  uint32_t word = 0;
+  const uint8_t segments = encodeCharacter(displayBuffer[digitIndex]);
+
+  for (uint8_t segment = 0; segment < kSegmentCount; ++segment) {
+    if ((segments >> segment) & 0x1U) {
+      word |= (1UL << (kSegmentStartBit + segment));
+    }
+  }
+
+  word |= (1UL << (kGridStartBit + digitIndex));
+  shiftDriverWord(word);
+}
+
+void renderIndicator() {
+  uint32_t word = 1UL << kIndicatorGridBit;
+  if (pointEnabled) {
+    word |= 1UL << kPointSegmentBit;
+  }
+  if (bellEnabled) {
+    word |= 1UL << kBellSegmentBit;
+  }
+  shiftDriverWord(word);
+}
+
+void begin() {
+  pinMode(kDataPin, OUTPUT);
+  pinMode(kClockPin, OUTPUT);
+  pinMode(kLatchPin, OUTPUT);
+  if (kBlankPin >= 0) {
+    pinMode(kBlankPin, OUTPUT);
+  }
+
+  digitalWrite(kDataPin, LOW);
+  digitalWrite(kClockPin, HIGH);
+  digitalWrite(kLatchPin, HIGH);
+  setBlanked(true);
+  writeText("0");
+  shiftDriverWord(0);
+}
+
+void refresh() {
+  setBlanked(true);
+  if (currentPhase < kDigitCount) {
+    renderDigit(currentPhase);
+  } else if (pointEnabled || bellEnabled) {
+    renderIndicator();
+  } else {
+    shiftDriverWord(0);
+  }
+  setBlanked(false);
+  delayMicroseconds(kDigitHoldMicros);
+  setBlanked(true);
+
+  currentPhase = (currentPhase + 1) % (kDigitCount + 1);
+}
+
+}  // namespace vfd
+
 struct GaugePins {
   uint8_t dirPin;
   uint8_t stepPin;
@@ -111,7 +318,8 @@ bool ledsDirty = false;
 //   LED <id> <idx|a-b> <r> <g> <b>
 //   BLINK <id> <idx|a-b> <on_ms> <off_ms> [<r> <g> <b>]
 //   BREATHE <id> <idx|a-b> <period_ms> <r> <g> <b>
-//   DFLASH <id> <idx|a-b> <r> <g> <b>
+//   DFLASH <id> <idx|a-b> <r> <ig> <b>
+//   VFD <text[.!]>
 //   PING
 //
 // Controller -> host replies / events:
@@ -136,6 +344,8 @@ bool ledsDirty = false;
 //     Sent by LED/BLINK/BREATHE/DFLASH when an LED index or range is invalid.
 //   ERR BAD_TIME
 //     Sent by BLINK/BREATHE when the timing parameter is invalid.
+//   ERR BAD_VFD
+//     Sent by VFD when the text payload is malformed.
 //   POS <id> <currentPos> <targetPos> <homed> <homingState> <sweepEnabled>
 //     Emitted once per gauge before the trailing OK reply to POS?.
 //   LED <id> <idx> <r> <g> <b>
@@ -594,6 +804,32 @@ bool parsePing(const String& line) {
   return false;
 }
 
+// Parses `VFD <text>` where <text> is up to four hex characters with optional `.` and `!` suffixes.
+// Replies: `OK`, `ERR BAD_VFD`.
+bool parseVfd(const String& line) {
+  if (line == "VFD") {
+    vfd::clear();
+    sendReply("OK");
+    return true;
+  }
+
+  if (!line.startsWith("VFD ")) return false;
+
+  const String payload = line.substring(4);
+  if (payload.length() == 0) {
+    vfd::clear();
+    sendReply("OK");
+    return true;
+  }
+
+  if (vfd::parseCommand(payload)) {
+    sendReply("OK");
+  } else {
+    sendReply("ERR BAD_VFD");
+  }
+  return true;
+}
+
 // Answers `LED?` with the current RGB values for every configured LED.
 // Emits one `LED <id> <idx> <r> <g> <b>` line per configured LED, then replies `OK`.
 bool parseLedQuery(const String& line) {
@@ -827,6 +1063,7 @@ void processLine(const String& line) {
   if (parseBlink(line)) return;
   if (parseBreathe(line)) return;
   if (parseDflash(line)) return;
+  if (parseVfd(line)) return;
   if (parsePing(line)) return;
 
   sendReply("ERR BAD_CMD");
@@ -885,6 +1122,8 @@ void setup() {
   FastLED.setBrightness(255);
   FastLED.show();
 
+  vfd::begin();
+
   requestHomeAll();
 
   DEBUG_PORT.println("READY");
@@ -895,6 +1134,7 @@ void setup() {
 // Main service loop: ingest commands, advance effects, move gauges, flush LEDs.
 void loop() {
   readCommands();
+  vfd::refresh();
   updateBlink();
 
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {

VFDStandalone/Pinout.md

@@ -0,0 +1,56 @@
+# Pinout
+
+This project uses an Arduino Mega 2560 with an `HV5812P` high-voltage shift register / latch driver.
+
+The sketch in [VFDStandalone.ino](/home/adebaumann/development/arduino_gauge_controller/VFDStandalone/VFDStandalone.ino:1) currently expects these logic connections.
+
+## Arduino Mega 2560 -> HV5812P
+
+| Mega Pin | Mega Function | HV5812P Signal | Notes |
+|---|---|---|---|
+| `D51` | `MOSI` | `DATA` / `DIN` | Serial data into the HV5812P |
+| `D52` | `SCK` | `CLOCK` / `CLK` | Shift clock |
+| `D53` | `SS` | `LATCH` / `STROBE` | Transfers shifted bits to the outputs |
+| `D49` | GPIO | `BLANK` / `OE` | Optional. Set `kHvBlankPin = -1` in the sketch if unused |
+| `GND` | Ground | Logic `GND` | Mega and HV5812P logic ground must be common |
+
+## HV5812P Outputs -> VFD Tube
+
+| HV5812P Output | Function |
+|---|---|
+| `HVOut1` | Segment `A` |
+| `HVOut2` | Segment `B` |
+| `HVOut3` | Segment `C` |
+| `HVOut4` | Segment `D` |
+| `HVOut5` | Segment `E` |
+| `HVOut6` | Segment `F` |
+| `HVOut7` | Segment `G` |
+| `HVOut8` | Decimal point segment |
+| `HVOut9` | Alarm bell segment |
+| `HVOut10` | Digit grid 1 |
+| `HVOut11` | Digit grid 2 |
+| `HVOut12` | Digit grid 3 |
+| `HVOut13` | Digit grid 4 |
+| `HVOut14` | Indicator grid between digits 2 and 3 |
+
+## Serial Input Format
+
+Examples supported by the sketch:
+
+- `1234` -> digits only
+- `1234.` -> decimal point on
+- `1234!` -> alarm bell on
+- `1234.!` -> decimal point and alarm bell on
+
+## Power and Safety Notes
+
+- The Arduino `5V` pin is for the logic side only.
+- The HV5812P also needs its required logic supply and high-voltage supply per the datasheet.
+- The VFD filament, grid, and segment high-voltage wiring are separate from the Arduino logic pins.
+- Do not connect any high-voltage VFD node directly to the Arduino Mega.
+- If the blanking behavior is inverted on your board, change `kBlankActiveHigh` in the sketch.
+
+## Important
+
+This file names the functional signals on the `HV5812P`, not the package pin numbers.
+If you want a package-pin wiring table too, I can add one once you confirm the exact datasheet variant / package orientation you are using.

VFDStandalone/VFDStandalone.ino

@@ -0,0 +1,342 @@
+// Arduino Mega 2560 + HV5812P VFD driver
+//
+// Tube wiring:
+// - HVOut1..HVOut7  -> digit segments A..G
+// - HVOut8          -> decimal point segment on the indicator grid
+// - HVOut9          -> alarm bell segment on the indicator grid
+// - HVOut10..HVOut13 -> digits 1..4
+// - HVOut14         -> indicator grid between digits 2 and 3
+//
+// Send an integer over the USB serial port and it will be shown on the VFD.
+// Examples:
+//   42<newline>
+//   -17<newline>
+//   1234.<newline>   // enables the decimal point
+//   1234!<newline>   // enables the alarm bell
+//   1234.!<newline>  // enables both
+
+#include <Arduino.h>
+
+namespace {
+
+constexpr uint8_t kHvDataPin = 51;   // MOSI on Mega 2560
+constexpr uint8_t kHvClockPin = 52;  // SCK on Mega 2560
+constexpr uint8_t kHvLatchPin = 53;  // User-configurable latch/strobe pin
+constexpr int8_t kHvBlankPin = 49;   // Set to -1 if BL/OE is not connected
+constexpr bool kBlankActiveHigh = true;
+
+constexpr unsigned long kSerialBaud = 115200;
+constexpr unsigned long kDigitHoldMicros = 2000;
+
+constexpr uint8_t kDigitCount = 4;
+constexpr uint8_t kSegmentCount = 7;
+constexpr uint8_t kDriverBits = 20;
+
+constexpr uint8_t kSegmentStartBit = 0;  // HVOut1 -> bit 0
+constexpr uint8_t kPointSegmentBit = 7;  // HVOut8 -> bit 7
+constexpr uint8_t kBellSegmentBit = 8;   // HVOut9 -> bit 8
+constexpr uint8_t kGridStartBit = 9;     // HVOut10 -> bit 9
+constexpr uint8_t kIndicatorGridBit = 13;  // HVOut14 -> bit 13
+
+char g_displayBuffer[kDigitCount] = {' ', ' ', ' ', ' '};
+char g_inputBuffer[16];
+uint8_t g_inputLength = 0;
+bool g_pointEnabled = false;
+bool g_bellEnabled = false;
+uint8_t g_rawOutput = 0;
+
+// Seven-segment encoding order is A, B, C, D, E, F, G.
+uint8_t encodeCharacter(char c) {
+  switch (c) {
+    case '0': return 0b0111111;
+    case '1': return 0b0000110;
+    case '2': return 0b1011011;
+    case '3': return 0b1001111;
+    case '4': return 0b1100110;
+    case '5': return 0b1101101;
+    case '6': return 0b1111101;
+    case '7': return 0b0000111;
+    case '8': return 0b1111111;
+    case '9': return 0b1101111;
+    case 'A':
+    case 'a': return 0b1110111;
+    case 'B':
+    case 'b': return 0b1111100;
+    case 'C':
+    case 'c': return 0b0111001;
+    case 'D':
+    case 'd': return 0b1011110;
+    case 'E':
+    case 'e': return 0b1111001;
+    case 'F':
+    case 'f': return 0b1110001;
+    case '-': return 0b1000000;
+    default: return 0;
+  }
+}
+
+void shiftDriverWord(uint32_t word) {
+  digitalWrite(kHvLatchPin, HIGH);
+  digitalWrite(kHvClockPin, HIGH);
+
+  for (int8_t bit = kDriverBits - 1; bit >= 0; --bit) {
+    digitalWrite(kHvDataPin, (word >> bit) & 0x1U ? HIGH : LOW);
+    digitalWrite(kHvClockPin, LOW);
+    digitalWrite(kHvClockPin, HIGH);
+  }
+
+  digitalWrite(kHvLatchPin, LOW);
+  digitalWrite(kHvLatchPin, HIGH);
+}
+
+void setDisplayBlanked(bool blanked) {
+  if (kHvBlankPin < 0) {
+    return;
+  }
+
+  const bool level = kBlankActiveHigh ? blanked : !blanked;
+  digitalWrite(kHvBlankPin, level ? HIGH : LOW);
+}
+
+void blankDisplay() {
+  shiftDriverWord(0);
+}
+
+uint32_t maskForHvOutput(uint8_t hvOutput) {
+  if (hvOutput == 0 || hvOutput > kDriverBits) {
+    return 0;
+  }
+
+  return 1UL << (hvOutput - 1);
+}
+
+void renderDigit(uint8_t digitIndex) {
+  uint32_t word = 0;
+  const uint8_t segments = encodeCharacter(g_displayBuffer[digitIndex]);
+
+  for (uint8_t segment = 0; segment < kSegmentCount; ++segment) {
+    if ((segments >> segment) & 0x1U) {
+      word |= (1UL << (kSegmentStartBit + segment));
+    }
+  }
+
+  word |= (1UL << (kGridStartBit + digitIndex));
+  shiftDriverWord(word);
+}
+
+void renderIndicator() {
+  uint32_t word = 1UL << kIndicatorGridBit;
+
+  if (g_pointEnabled) {
+    word |= 1UL << kPointSegmentBit;
+  }
+
+  if (g_bellEnabled) {
+    word |= 1UL << kBellSegmentBit;
+  }
+
+  shiftDriverWord(word);
+}
+
+void writeTextToDisplay(const char* text) {
+  for (uint8_t i = 0; i < kDigitCount; ++i) {
+    g_displayBuffer[i] = ' ';
+  }
+
+  size_t len = strlen(text);
+  if (len > kDigitCount) {
+    text += len - kDigitCount;
+    len = kDigitCount;
+  }
+
+  const uint8_t start = kDigitCount - len;
+  for (uint8_t i = 0; i < len; ++i) {
+    g_displayBuffer[start + i] = text[i];
+  }
+}
+
+void setDisplayFromNumber(long value) {
+  char buffer[16];
+  ltoa(value, buffer, 10);
+  writeTextToDisplay(buffer);
+}
+
+bool parseDisplayCommand(const char* input,
+                         char* displayText,
+                         size_t displayTextSize,
+                         bool& pointEnabled,
+                         bool& bellEnabled) {
+  size_t inputIndex = 0;
+  size_t displayIndex = 0;
+
+  if (input[inputIndex] == '-') {
+    if (displayIndex + 1 >= displayTextSize) {
+      return false;
+    }
+    displayText[displayIndex++] = input[inputIndex++];
+  }
+
+  const size_t digitStart = inputIndex;
+  while (isxdigit(static_cast<unsigned char>(input[inputIndex]))) {
+    if (displayIndex + 1 >= displayTextSize) {
+      return false;
+    }
+    displayText[displayIndex] = toupper(static_cast<unsigned char>(input[inputIndex]));
+    ++displayIndex;
+    ++inputIndex;
+  }
+
+  if (inputIndex == digitStart) {
+    return false;
+  }
+
+  pointEnabled = false;
+  bellEnabled = false;
+
+  while (input[inputIndex] != '\0') {
+    if (input[inputIndex] == '.') {
+      pointEnabled = true;
+    } else if (input[inputIndex] == '!') {
+      bellEnabled = true;
+    } else {
+      return false;
+    }
+    ++inputIndex;
+  }
+
+  displayText[displayIndex] = '\0';
+  return true;
+}
+
+bool parseRawOutputCommand(const char* input, uint8_t& hvOutput) {
+  if (strncmp(input, "RAW ", 4) != 0) {
+    return false;
+  }
+
+  char* endPtr = nullptr;
+  const long parsed = strtol(input + 4, &endPtr, 10);
+  if (*endPtr != '\0' || parsed < 0 || parsed > kDriverBits) {
+    return false;
+  }
+
+  hvOutput = static_cast<uint8_t>(parsed);
+  return true;
+}
+
+void commitSerialBuffer() {
+  if (g_inputLength == 0) {
+    return;
+  }
+
+  g_inputBuffer[g_inputLength] = '\0';
+
+  uint8_t rawOutput = 0;
+  if (parseRawOutputCommand(g_inputBuffer, rawOutput)) {
+    g_rawOutput = rawOutput;
+    if (g_rawOutput == 0) {
+      Serial.println(F("RAW mode OFF"));
+    } else {
+      Serial.print(F("RAW mode: HVOUT"));
+      Serial.println(g_rawOutput);
+    }
+    g_inputLength = 0;
+    return;
+  }
+
+  char displayText[16];
+  bool pointEnabled = false;
+  bool bellEnabled = false;
+  if (parseDisplayCommand(g_inputBuffer, displayText, sizeof(displayText), pointEnabled, bellEnabled)) {
+    g_rawOutput = 0;
+    writeTextToDisplay(displayText);
+    g_pointEnabled = pointEnabled;
+    g_bellEnabled = bellEnabled;
+    Serial.print(F("Displaying: "));
+    Serial.println(displayText);
+    Serial.print(F("Point: "));
+    Serial.println(g_pointEnabled ? F("ON") : F("OFF"));
+    Serial.print(F("Bell: "));
+    Serial.println(g_bellEnabled ? F("ON") : F("OFF"));
+  } else {
+    Serial.print(F("Ignored invalid input: "));
+    Serial.println(g_inputBuffer);
+  }
+
+  g_inputLength = 0;
+}
+
+void pollSerial() {
+  while (Serial.available() > 0) {
+    const char incoming = static_cast<char>(Serial.read());
+
+    if (incoming == '\r' || incoming == '\n') {
+      commitSerialBuffer();
+      continue;
+    }
+
+    if (incoming == '\b' || incoming == 127) {
+      if (g_inputLength > 0) {
+        --g_inputLength;
+      }
+      continue;
+    }
+
+    if (g_inputLength < sizeof(g_inputBuffer) - 1) {
+      g_inputBuffer[g_inputLength++] = incoming;
+    }
+  }
+}
+
+void refreshDisplay() {
+  if (g_rawOutput != 0) {
+    setDisplayBlanked(true);
+    shiftDriverWord(maskForHvOutput(g_rawOutput));
+    setDisplayBlanked(false);
+    delayMicroseconds(kDigitHoldMicros);
+    return;
+  }
+
+  static uint8_t currentPhase = 0;
+
+  setDisplayBlanked(true);
+  if (currentPhase < kDigitCount) {
+    renderDigit(currentPhase);
+  } else if (g_pointEnabled || g_bellEnabled) {
+    renderIndicator();
+  } else {
+    blankDisplay();
+  }
+  setDisplayBlanked(false);
+  delayMicroseconds(kDigitHoldMicros);
+  setDisplayBlanked(true);
+
+  currentPhase = (currentPhase + 1) % (kDigitCount + 1);
+}
+
+}  // namespace
+
+void setup() {
+  pinMode(kHvDataPin, OUTPUT);
+  pinMode(kHvClockPin, OUTPUT);
+  pinMode(kHvLatchPin, OUTPUT);
+  if (kHvBlankPin >= 0) {
+    pinMode(kHvBlankPin, OUTPUT);
+  }
+
+  digitalWrite(kHvDataPin, LOW);
+  digitalWrite(kHvClockPin, HIGH);
+  digitalWrite(kHvLatchPin, HIGH);
+  setDisplayBlanked(true);
+
+  Serial.begin(kSerialBaud);
+  writeTextToDisplay("0");
+  blankDisplay();
+
+  Serial.println(F("HV5812P VFD controller ready."));
+  Serial.println(F("Send an integer followed by newline."));
+}
+
+void loop() {
+  pollSerial();
+  refreshDisplay();
+}

gauge.py

@@ -756,10 +756,10 @@ def connect_mqtt():
 
 _mqtt_check_interval_ms = 30000
 _last_mqtt_check = 0
-_discovery_phases = ()
-_discovery_phase_idx = 0
+_discovery_queue = []
+_discovery_idx = 0
 _last_discovery_ms = 0
-_DISCOVERY_INTERVAL_MS = 500
+_DISCOVERY_INTERVAL_MS = 200
 
 
 def check_mqtt():
@@ -809,23 +809,16 @@ def check_mqtt():
     return False
 
 
-def _discovery_pause(client, count=5, delay_ms=25):
-    for _ in range(count):
-        client.check_msg()
-        utime.sleep_ms(delay_ms)
-    gc.collect()
-
-
 def _publish_discovery_entity(client, topic, payload, log_msg):
     client.publish(topic, ujson.dumps(payload), retain=True)
     info(log_msg)
 
 
-def _publish_gauge_discovery(client, dev_ref):
+def _append_gauge_discovery(entries, dev_ref):
     for i, g in enumerate(gauges):
         gt = gauge_topics[i]
-        _publish_discovery_entity(
-            client,
+        entries.append(
+            (
                 gt["disc"],
                 {
                     "name": g["entity_name"],
@@ -842,14 +835,14 @@ def _publish_gauge_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} ({g['name']})",
             )
-    _discovery_pause(client)
+        )
 
 
-def _publish_speed_discovery(client, dev_ref):
+def _append_speed_discovery(entries, dev_ref):
     for i, g in enumerate(gauges):
         gt = gauge_topics[i]
-        _publish_discovery_entity(
-            client,
+        entries.append(
+            (
                 gt["speed_disc"],
                 {
                     "name": f"{g['name']} Speed",
@@ -868,14 +861,14 @@ def _publish_speed_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} speed",
             )
-    _discovery_pause(client)
+        )
 
 
-def _publish_acceleration_discovery(client, dev_ref):
+def _append_acceleration_discovery(entries, dev_ref):
     for i, g in enumerate(gauges):
         gt = gauge_topics[i]
-        _publish_discovery_entity(
-            client,
+        entries.append(
+            (
                 gt["acceleration_disc"],
                 {
                     "name": f"{g['name']} Acceleration",
@@ -894,14 +887,14 @@ def _publish_acceleration_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} acceleration",
             )
-    _discovery_pause(client)
+        )
 
 
-def _publish_indicator_led_discovery(client, dev_ref):
+def _append_indicator_led_discovery(entries, dev_ref):
     for i, g in enumerate(gauges):
         gt = gauge_topics[i]
-        _publish_discovery_entity(
-            client,
+        entries.append(
+            (
                 gt["led_red_disc"],
                 {
                     "name": f"{g['name']} Dial Red LED",
@@ -918,9 +911,9 @@ def _publish_indicator_led_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} red LED",
             )
-
-        _publish_discovery_entity(
-            client,
+        )
+        entries.append(
+            (
                 gt["led_green_disc"],
                 {
                     "name": f"{g['name']} Dial Green LED",
@@ -937,14 +930,14 @@ def _publish_indicator_led_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} green LED",
             )
-    _discovery_pause(client)
+        )
 
 
-def _publish_backlight_status_discovery(client, dev_ref):
+def _append_backlight_status_discovery(entries, dev_ref):
     for i, g in enumerate(gauges):
         gt = gauge_topics[i]
-        _publish_discovery_entity(
-            client,
+        entries.append(
+            (
                 gt["led_bl_disc"],
                 {
                     "name": f"{g['name']} Backlight",
@@ -961,9 +954,9 @@ def _publish_backlight_status_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} backlight",
             )
-
-        _publish_discovery_entity(
-            client,
+        )
+        entries.append(
+            (
                 gt["status_red_disc"],
                 {
                     "name": f"{g['name']} Channel Status Red",
@@ -980,9 +973,9 @@ def _publish_backlight_status_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} status red",
             )
-
-        _publish_discovery_entity(
-            client,
+        )
+        entries.append(
+            (
                 gt["status_green_disc"],
                 {
                     "name": f"{g['name']} Channel Status Green",
@@ -999,47 +992,25 @@ def _publish_backlight_status_discovery(client, dev_ref):
                 },
                 f"Discovery: gauge {i} status green",
             )
-    _discovery_pause(client)
-
-
-def publish_discovery(client):
-    """Publish all HA MQTT discovery payloads for gauges and LEDs."""
-    _dev_ref = _DEVICE
-
-    # Clear any previously registered switch entities (migration to light).
-    for i in range(num_gauges):
-        for old_t in [
-            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_red/config",
-            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_green/config",
-            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_status_red/config",
-            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_status_green/config",
-        ]:
-            client.publish(old_t, b"", retain=True)
-        _discovery_pause(client, count=2, delay_ms=15)
-
-    _publish_gauge_discovery(client, _dev_ref)
-    _publish_speed_discovery(client, _dev_ref)
-    _publish_acceleration_discovery(client, _dev_ref)
-    _publish_indicator_led_discovery(client, _dev_ref)
-    _publish_backlight_status_discovery(client, _dev_ref)
+        )
 
 
 def schedule_discovery():
-    global _discovery_phases, _discovery_phase_idx, _last_discovery_ms
+    global _discovery_queue, _discovery_idx, _last_discovery_ms
     _dev_ref = _DEVICE
-    _discovery_phases = (
-        lambda client: _clear_legacy_discovery(client),
-        lambda client: _publish_gauge_discovery(client, _dev_ref),
-        lambda client: _publish_speed_discovery(client, _dev_ref),
-        lambda client: _publish_acceleration_discovery(client, _dev_ref),
-        lambda client: _publish_indicator_led_discovery(client, _dev_ref),
-        lambda client: _publish_backlight_status_discovery(client, _dev_ref),
-    )
-    _discovery_phase_idx = 0
+    entries = []
+    _append_legacy_discovery(entries)
+    _append_gauge_discovery(entries, _dev_ref)
+    _append_speed_discovery(entries, _dev_ref)
+    _append_acceleration_discovery(entries, _dev_ref)
+    _append_indicator_led_discovery(entries, _dev_ref)
+    _append_backlight_status_discovery(entries, _dev_ref)
+    _discovery_queue = entries
+    _discovery_idx = 0
     _last_discovery_ms = 0
 
 
-def _clear_legacy_discovery(client):
+def _append_legacy_discovery(entries):
     for i in range(num_gauges):
         for old_t in [
             f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_red/config",
@@ -1047,22 +1018,27 @@ def _clear_legacy_discovery(client):
             f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_status_red/config",
             f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_status_green/config",
         ]:
-            client.publish(old_t, b"", retain=True)
-        _discovery_pause(client, count=2, delay_ms=15)
+            entries.append((old_t, b"", None))
 
 
 def service_discovery():
-    global _discovery_phase_idx, _last_discovery_ms
-    if client_ref is None or _discovery_phase_idx >= len(_discovery_phases):
+    global _discovery_idx, _last_discovery_ms
+    if client_ref is None or _discovery_idx >= len(_discovery_queue):
         return
 
     now = utime.ticks_ms()
     if _last_discovery_ms and utime.ticks_diff(now, _last_discovery_ms) < _DISCOVERY_INTERVAL_MS:
         return
 
-    _discovery_phases[_discovery_phase_idx](client_ref)
-    _discovery_phase_idx += 1
+    topic, payload, log_msg = _discovery_queue[_discovery_idx]
+    if isinstance(payload, bytes):
+        client_ref.publish(topic, payload, retain=True)
+    else:
+        _publish_discovery_entity(client_ref, topic, payload, log_msg)
+    _discovery_idx += 1
     _last_discovery_ms = utime.ticks_ms()
+    if (_discovery_idx & 3) == 0:
+        gc.collect()
 
 
 def publish_online(client):