VFD functionality successfully integrated into Gaugecontroller

CLAUDE.md

@@ -4,37 +4,47 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co
 
 ## Build & Upload
 
-This is a single-file Arduino sketch (`Gaugecontroller.ino`). Requires the **FastLED** library (`arduino-cli lib install FastLED`). Use the Arduino IDE or `arduino-cli`:
+Main firmware lives in `Gaugecontroller/Gaugecontroller.ino`. Requires the **FastLED** library (`arduino-cli lib install FastLED`). Use the Arduino IDE or `arduino-cli`:
 
 ```bash
 # Compile (replace board/port as needed)
-arduino-cli compile --fqbn arduino:avr:mega Gaugecontroller.ino
+arduino-cli compile --fqbn arduino:avr:mega Gaugecontroller
 
 # Upload
-arduino-cli upload -p /dev/ttyACM0 --fqbn arduino:avr:mega Gaugecontroller.ino
+arduino-cli upload -p /dev/ttyACM0 --fqbn arduino:avr:mega Gaugecontroller
 ```
 
-Serial monitor: 115200 baud (`Serial` is both CMD_PORT and DEBUG_PORT).
+Current default serial setup: `CMD_PORT` and `DEBUG_PORT` both point to `Serial1` at 38400 baud.
 
 ## Switching serial ports (debug → production)
 
 Two `#define`s at the top of `Gaugecontroller.ino` control where commands and debug output go:
 
 ```cpp
-#define CMD_PORT   Serial   // command channel (host sends SET, HOME, etc.)
+#define CMD_PORT   Serial1  // command channel (host sends SET, HOME, etc.)
-#define DEBUG_PORT Serial   // diagnostic prints (homing, boot messages)
+#define DEBUG_PORT Serial1  // diagnostic prints (homing, boot messages)
 ```
 
-**Debug / USB-only (default):** both point to `Serial` (the USB-CDC port). Connect via `minicom` or the Arduino IDE serial monitor at 115200 baud.
+**Current default:** both point to `Serial1`, so command and debug traffic share Mega pins TX1=18 / RX1=19 at 38400 baud.
 
-**Production (hardware UART):** change `CMD_PORT` to a hardware serial port so a host MCU or Raspberry Pi can drive it without occupying the USB port:
+**USB-only debug setup:** point both defines back at `Serial` if you want to talk to the sketch over the Arduino USB port instead:
 
 ```cpp
-#define CMD_PORT   Serial1  // TX1=pin18, RX1=pin19
+#define CMD_PORT   Serial
-#define DEBUG_PORT Serial   // keep USB for monitoring, or silence it (see below)
+#define DEBUG_PORT Serial
 ```
 
-Arduino Mega hardware UARTs:
+At that point the matching `begin()` call in `setup()` also needs to use the same baud rate you expect on the host side.
+
+**Split command/debug ports:** if `CMD_PORT` and `DEBUG_PORT` do not point to the same serial port, `setup()` must initialise both. Right now it only calls:
+
+```cpp
+DEBUG_PORT.begin(38400);
+```
+
+If you split them, add a second `CMD_PORT.begin(...)` call.
+
+Arduino Mega hardware UARTs for reference:
 
 | Port    | TX pin | RX pin |
 |---------|--------|--------|
@@ -42,14 +52,6 @@ Arduino Mega hardware UARTs:
 | Serial2 | 16     | 17     |
 | Serial3 | 14     | 15     |
 
-`setup()` calls `DEBUG_PORT.begin(115200)` only. If `CMD_PORT` differs from `DEBUG_PORT` you must also begin it — add a second `begin` call in `setup()`:
-
-```cpp
-CMD_PORT.begin(115200);
-```
-
-**Silencing debug output entirely:** point `DEBUG_PORT` at a null stream, or wrap all `DEBUG_PORT` calls in an `#ifdef DEBUG` guard. The simplest option is to replace the define with a no-op object, but the easiest production approach is just to leave `DEBUG_PORT Serial` and ignore the USB output.
-
 ## Architecture
 
 The sketch controls `GAUGE_COUNT` stepper-motor gauges using a trapezoidal velocity profile and a simple text serial protocol.
@@ -74,7 +76,7 @@ When `sweepEnabled`, `updateSweepTarget` bounces `targetPos` between `minPos` an
 
 ### LED strip
 
-One shared WS2812B strip is driven from `LED_DATA_PIN` (default 6). Each gauge owns a contiguous segment of the strip; `gaugePins[i].ledCount` sets the segment length (0 = no LEDs). `TOTAL_LEDS` is computed at compile time via `constexpr sumLedCounts()` — no manual constant to keep in sync. Per-gauge offsets into the flat `leds[]` array are computed once in `setup()` into `gaugeLedOffset[]`. `FastLED.show()` is called immediately after each `LED` command.
+One shared WS2812B strip is driven from `LED_DATA_PIN` (currently 22). Each gauge owns a contiguous segment of the strip; `gaugePins[i].ledCount` sets the segment length (0 = no LEDs). `TOTAL_LEDS` is computed at compile time via `constexpr sumLedCounts()` — no manual constant to keep in sync. Per-gauge offsets into the flat `leds[]` array are computed once in `setup()` into `gaugeLedOffset[]`. LED commands and effects mark the strip dirty, and `FastLED.show()` is called once per main-loop iteration if anything changed.
 
 ### Serial command protocol
 

Gaugecontroller/Gaugecontroller.ino

@@ -1,32 +1,239 @@
 #include <Arduino.h>
+#include <ctype.h>
 #include <math.h>
 #include <FastLED.h>
 
-static const uint8_t GAUGE_COUNT = 2;
+static const uint8_t GAUGE_COUNT = 3;
 
-// LED strip — one shared WS2812B strip, segmented per gauge.
+// One shared WS2812B strip, split into per-gauge segments.
-// Set LED_DATA_PIN to the digital pin driving the strip data line.
-// TOTAL_LEDS is computed automatically from gaugePins[].ledCount.
 static const uint8_t LED_DATA_PIN = 22;
 
-// For now: commands come over USB serial
+// For now, command and debug traffic share the same serial port.
 #define CMD_PORT Serial1
 #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;
-  int8_t enablePin;          // -1 if unused
+  int8_t enablePin;          // -1 means there is no enable pin
   bool dirInverted;
   bool stepActiveHigh;
   bool enableActiveLow;
-  uint8_t ledCount;          // WS2812B LEDs on this gauge's strip segment (0 if none)
+  uint8_t ledCount;          // LEDs assigned to this gauge
 };
 
 constexpr GaugePins gaugePins[GAUGE_COUNT] = {
   // dir, step, en, dirInv, stepHigh, enActiveLow, leds
   {50, 51, -1, false, true, true, 7},   // Gauge 0
   {8,  9,  -1, true,  true, true, 7},   // Gauge 1
+  {52, 53, -1, false, true, true, 7},   // Gauge 2
 };
 
 constexpr uint8_t sumLedCounts(uint8_t i = 0) {
@@ -47,8 +254,8 @@ struct Gauge {
   long targetPos = 0;
 
   long minPos = 0;
-  long maxPos = 3780;             // adjust to your usable travel
+  long maxPos = 3780;
-  long homingBackoffSteps = 3800; // should exceed reverse travel slightly
+  long homingBackoffSteps = 3800; // Deliberately a touch past full reverse travel.
 
   float velocity = 0.0f;
   float maxSpeed = 5000.0f;
@@ -77,14 +284,11 @@ struct BlinkState {
   LedFx         fx          = FX_BLINK;
   CRGB          onColor;
   unsigned long lastMs      = 0;
-  // FX_BLINK
   uint16_t      onMs        = 500;
   uint16_t      offMs       = 500;
   bool          currentlyOn = false;
-  // FX_BREATHE: smooth triangle-wave fade
   uint16_t      periodMs    = 2000;
   uint16_t      cyclePos    = 0;
-  // FX_DFLASH: two quick flashes then pause
   uint8_t       dphase      = 0;
 };
 
@@ -96,14 +300,68 @@ uint8_t gaugeLedOffset[GAUGE_COUNT];
 BlinkState blinkState[TOTAL_LEDS];
 bool ledsDirty = false;
 
+// Sends one-line command replies back over the control port.
+//
+// Serial protocol summary.
+//
+// Host -> controller commands (newline-terminated ASCII):
+//   SET <id> <pos>
+//   SPEED <id> <steps_per_s>
+//   ACCEL <id> <steps_per_s2>
+//   ENABLE <id> <0|1>
+//   ZERO <id>
+//   HOME <id>
+//   HOMEALL
+//   SWEEP <id> <accel> <speed>
+//   POS?
+//   LED?
+//   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> <ig> <b>
+//   VFD <text[.!]>
+//   PING
+//
+// Controller -> host replies / events:
+//   READY
+//     Sent once from setup() after boot completes.
+//   OK
+//     Sent after a valid mutating command, and after POS?/LED? once all data lines
+//     for that query have been emitted.
+//   PONG
+//     Sent in response to PING.
+//   ERR BAD_CMD
+//     Sent when a complete line matches no parser.
+//   ERR TOO_LONG
+//     Sent when an input line exceeds the receive buffer limit.
+//   ERR BAD_ID
+//     Sent by commands that take a gauge id when the id is outside 0..GAUGE_COUNT-1.
+//   ERR BAD_SPEED
+//     Sent by SPEED when the requested speed is <= 0.
+//   ERR BAD_ACCEL
+//     Sent by ACCEL when the requested acceleration is <= 0.
+//   ERR BAD_IDX
+//     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>
+//     Emitted once per configured LED before the trailing OK reply to LED?.
+//   HOMED <id>
+//     Debug event printed on DEBUG_PORT when a homing sequence settles successfully.
 void sendReply(const String& s) {
   CMD_PORT.println(s);
 }
 
+// Tiny float absolute-value helper to avoid dragging more machinery into the sketch.
 float absf(float x) {
   return (x < 0.0f) ? -x : x;
 }
 
+// Updates the cached enable state and toggles the hardware pin if one exists.
 void setEnable(uint8_t id, bool en) {
   if (id >= GAUGE_COUNT) return;
   gauges[id].enabled = en;
@@ -115,11 +373,13 @@ void setEnable(uint8_t id, bool en) {
   digitalWrite(pin, level ? HIGH : LOW);
 }
 
+// Applies the logical direction after accounting for per-gauge inversion.
 void setDir(uint8_t id, bool forward) {
   bool level = gaugePins[id].dirInverted ? !forward : forward;
   digitalWrite(gaugePins[id].dirPin, level ? HIGH : LOW);
 }
 
+// Emits one step pulse with the polarity expected by the driver.
 void pulseStep(uint8_t id) {
   bool active = gaugePins[id].stepActiveHigh;
   digitalWrite(gaugePins[id].stepPin, active ? HIGH : LOW);
@@ -127,6 +387,7 @@ void pulseStep(uint8_t id) {
   digitalWrite(gaugePins[id].stepPin, active ? LOW : HIGH);
 }
 
+// Moves the motor by one step if the requested direction is still within allowed travel.
 void doStep(uint8_t id, int dir, bool allowPastMin = false) {
   Gauge& g = gauges[id];
   if (!g.enabled) return;
@@ -144,6 +405,7 @@ void doStep(uint8_t id, int dir, bool allowPastMin = false) {
   }
 }
 
+// Arms the homing state machine for one gauge and clears any in-flight motion.
 void requestHome(uint8_t id) {
   if (id >= GAUGE_COUNT) return;
   Gauge& g = gauges[id];
@@ -154,12 +416,14 @@ void requestHome(uint8_t id) {
   g.sweepEnabled = false;
 }
 
+// Starts the same homing sequence on every configured gauge.
 void requestHomeAll() {
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
     requestHome(i);
   }
 }
 
+// Advances the simple homing state machine until the gauge is parked at logical zero.
 void updateHoming(uint8_t id) {
   Gauge& g = gauges[id];
   unsigned long nowUs = micros();
@@ -170,6 +434,7 @@ void updateHoming(uint8_t id) {
       return;
 
     case HS_START:
+      // No endstop here; homing just walks back far enough to hit the hard stop.
       g.velocity = 0.0f;
       g.stepAccumulator = 0.0f;
       g.homingStepsRemaining = g.homingBackoffSteps;
@@ -213,6 +478,7 @@ void updateHoming(uint8_t id) {
   }
 }
 
+// Flips the sweep destination when the gauge has settled at either end of travel.
 void updateSweepTarget(uint8_t id) {
   Gauge& g = gauges[id];
   if (!g.sweepEnabled || !g.homed || g.homingState != HS_IDLE) return;
@@ -232,6 +498,7 @@ void updateSweepTarget(uint8_t id) {
   }
 }
 
+// Runs one gauge worth of motion control, including homing and optional sweeping.
 void updateGauge(uint8_t id) {
   Gauge& g = gauges[id];
 
@@ -266,6 +533,7 @@ void updateGauge(uint8_t id) {
   }
 
   float dir = (error > 0) ? 1.0f : (error < 0 ? -1.0f : 0.0f);
+  // Basic trapezoidal profile: brake if the remaining travel is shorter than the stop distance.
   float brakingDistance = (g.velocity * g.velocity) / (2.0f * g.accel + 0.0001f);
 
   if ((float)labs(error) <= brakingDistance) {
@@ -286,6 +554,7 @@ void updateGauge(uint8_t id) {
     g.velocity = dir * 5.0f;
   }
 
+  // Integrate fractional steps until there is enough to emit a real pulse.
   g.stepAccumulator += g.velocity * dt;
 
   while (g.stepAccumulator >= 1.0f) {
@@ -327,6 +596,8 @@ void updateGauge(uint8_t id) {
   }
 }
 
+// Parses `SET <id> <pos>` and updates the target position.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseSet(const String& line) {
   int id;
   long pos;
@@ -346,6 +617,8 @@ bool parseSet(const String& line) {
   return false;
 }
 
+// Parses `SPEED <id> <speed>` and updates the max step rate.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_SPEED`.
 bool parseSpeed(const String& line) {
   int firstSpace = line.indexOf(' ');
   int secondSpace = line.indexOf(' ', firstSpace + 1);
@@ -369,6 +642,8 @@ bool parseSpeed(const String& line) {
   return true;
 }
 
+// Parses `ACCEL <id> <accel>` and updates the acceleration limit.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_ACCEL`.
 bool parseAccel(const String& line) {
   int firstSpace = line.indexOf(' ');
   int secondSpace = line.indexOf(' ', firstSpace + 1);
@@ -392,6 +667,8 @@ bool parseAccel(const String& line) {
   return true;
 }
 
+// Parses `ENABLE <id> <0|1>` and toggles the selected driver.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseEnable(const String& line) {
   int id, en;
   if (sscanf(line.c_str(), "ENABLE %d %d", &id, &en) == 2) {
@@ -407,6 +684,8 @@ bool parseEnable(const String& line) {
   return false;
 }
 
+// Parses `ZERO <id>` and declares the current position to be home.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseZero(const String& line) {
   int id;
   if (sscanf(line.c_str(), "ZERO %d", &id) == 1) {
@@ -428,6 +707,8 @@ bool parseZero(const String& line) {
   return false;
 }
 
+// Parses `HOME <id>` or `HOMEALL` and kicks off the homing sequence.
+// Replies: `OK`, `ERR BAD_ID`. Successful completion later emits debug line `HOMED <id>`.
 bool parseHome(const String& line) {
   int id;
   if (sscanf(line.c_str(), "HOME %d", &id) == 1) {
@@ -450,6 +731,8 @@ bool parseHome(const String& line) {
   return false;
 }
 
+// Parses `SWEEP <id> <accel> <speed>` and enables or disables end-to-end motion.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseSweep(const String& line) {
   int firstSpace = line.indexOf(' ');
   int secondSpace = line.indexOf(' ', firstSpace + 1);
@@ -486,6 +769,9 @@ bool parseSweep(const String& line) {
   return true;
 }
 
+// Answers `POS?` with current motion state for every gauge.
+// Emits one `POS <id> <cur> <tgt> <homed> <homingState> <sweep>` line per gauge,
+// then replies `OK`.
 bool parsePosQuery(const String& line) {
   if (line == "POS?") {
     for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
@@ -508,6 +794,8 @@ bool parsePosQuery(const String& line) {
   return false;
 }
 
+// Answers the mandatory life question: are you there?
+// Reply: `PONG`.
 bool parsePing(const String& line) {
   if (line == "PING") {
     sendReply("PONG");
@@ -516,6 +804,34 @@ 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) {
   if (line == "LED?") {
     for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
@@ -539,6 +855,8 @@ bool parseLedQuery(const String& line) {
   return false;
 }
 
+// Parses `LED <id> <idx|a-b> <r> <g> <b>` and writes static colours.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`.
 bool parseLed(const String& line) {
   int id, r, g, b;
   char idxToken[16];
@@ -562,12 +880,12 @@ bool parseLed(const String& line) {
   return false;
 }
 
+// Parses `BLINK ...` and assigns a simple on/off effect to one LED or a range.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`, `ERR BAD_TIME`.
 bool parseBlink(const String& line) {
   int id, onMs, offMs, r, g, b;
   char idxToken[16];
-  // Accept both forms:
+  // Optional RGB values let BLINK either reuse or replace the current colour.
-  //   BLINK <id> <idx> <on_ms> <off_ms>           — use current LED colour
-  //   BLINK <id> <idx> <on_ms> <off_ms> <r> <g> <b> — set colour in same command
   int count = sscanf(line.c_str(), "BLINK %d %15s %d %d %d %d %d",
                      &id, idxToken, &onMs, &offMs, &r, &g, &b);
   if (count != 4 && count != 7) return false;
@@ -590,7 +908,7 @@ bool parseBlink(const String& line) {
 
   CRGB color = (count == 7)
     ? CRGB(constrain(r, 0, 255), constrain(g, 0, 255), constrain(b, 0, 255))
-    : CRGB(0, 0, 0);  // placeholder; overwritten per-LED below when count==4
+    : CRGB(0, 0, 0);  // Placeholder; replaced with the live LED colour below.
 
   unsigned long nowMs = millis();
   for (int i = idxFirst; i <= idxLast; i++) {
@@ -610,6 +928,8 @@ bool parseBlink(const String& line) {
   return true;
 }
 
+// Parses `BREATHE ...` and assigns a triangle-wave fade effect.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`, `ERR BAD_TIME`.
 bool parseBreathe(const String& line) {
   int id, periodMs, r, g, b;
   char idxToken[16];
@@ -641,6 +961,8 @@ bool parseBreathe(const String& line) {
   return true;
 }
 
+// Parses `DFLASH ...` and assigns the double-flash pattern.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`.
 bool parseDflash(const String& line) {
   int id, r, g, b;
   char idxToken[16];
@@ -670,6 +992,7 @@ bool parseDflash(const String& line) {
   return true;
 }
 
+// Advances all active LED effects and marks the strip dirty when something changed.
 void updateBlink() {
   unsigned long nowMs = millis();
   bool changed = false;
@@ -697,6 +1020,7 @@ void updateBlink() {
           uint32_t newPos = (uint32_t)bs.cyclePos + dt;
           bs.cyclePos = (uint16_t)(newPos % bs.periodMs);
           bs.lastMs = nowMs;
+          // Cheap triangle wave. It does the job and nobody has complained yet.
           uint16_t half = bs.periodMs >> 1;
           uint8_t bri = (bs.cyclePos < half)
               ? (uint8_t)((uint32_t)bs.cyclePos * 255 / half)
@@ -707,7 +1031,7 @@ void updateBlink() {
           break;
         }
         case FX_DFLASH: {
-          static const uint16_t dur[4] = {100, 100, 100, 700};
+          static const uint16_t dur[4] = {100, 100, 100, 700}; // on, off, on, longer off
           if ((nowMs - bs.lastMs) >= dur[bs.dphase]) {
             bs.lastMs = nowMs;
             bs.dphase = (bs.dphase + 1) & 3;
@@ -723,6 +1047,8 @@ void updateBlink() {
   if (changed) ledsDirty = true;
 }
 
+// Runs the command parsers in order until one claims the line.
+// Reply: `ERR BAD_CMD` when no parser accepts the line.
 void processLine(const String& line) {
   if (parseSet(line)) return;
   if (parseSpeed(line)) return;
@@ -737,11 +1063,14 @@ 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");
 }
 
+// Reads newline-delimited commands from serial and hands complete lines to the parser.
+// Reply: `ERR TOO_LONG` when the buffered line exceeds the receive limit before newline.
 void readCommands() {
   while (CMD_PORT.available()) {
     char c = (char)CMD_PORT.read();
@@ -762,8 +1091,10 @@ void readCommands() {
   }
 }
 
+// Initialises pins, LED bookkeeping and the initial homing cycle.
+// Reply/event: emits `READY` on CMD_PORT once boot is complete.
 void setup() {
-  DEBUG_PORT.begin(38400);
+  DEBUG_PORT.begin(SERIAL_BAUD);
   DEBUG_PORT.println("Gauge controller booting");
 
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
@@ -781,7 +1112,7 @@ void setup() {
     gauges[i].lastUpdateMicros = micros();
   }
 
-  // Compute per-gauge LED offsets and initialise the strip.
+  // Flatten the per-gauge LED counts into offsets on the shared strip.
   uint8_t ledOff = 0;
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
     gaugeLedOffset[i] = ledOff;
@@ -791,22 +1122,29 @@ void setup() {
   FastLED.setBrightness(255);
   FastLED.show();
 
+  vfd::begin();
+
   requestHomeAll();
 
   DEBUG_PORT.println("READY");
+  // Boot-complete handshake for the command channel.
   sendReply("READY");
 }
 
+// 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++) {
+    updateGauge(i);
+  }
+  
   if (ledsDirty) {
     FastLED.show();
     ledsDirty = false;
   }
 
-  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
+
-    updateGauge(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_queue = []
-_discovery_phase_idx = 0
+_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(
+        entries.append(
-            client,
+            (
                 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(
+        entries.append(
-            client,
+            (
                 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(
+        entries.append(
-            client,
+            (
                 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(
+        entries.append(
-            client,
+            (
                 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(
+        entries.append(
-            client,
+            (
                 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(
+        entries.append(
-            client,
+            (
                 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(
+        entries.append(
-            client,
+            (
                 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(
+        entries.append(
-            client,
+            (
                 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 = (
+    entries = []
-        lambda client: _clear_legacy_discovery(client),
+    _append_legacy_discovery(entries)
-        lambda client: _publish_gauge_discovery(client, _dev_ref),
+    _append_gauge_discovery(entries, _dev_ref)
-        lambda client: _publish_speed_discovery(client, _dev_ref),
+    _append_speed_discovery(entries, _dev_ref)
-        lambda client: _publish_acceleration_discovery(client, _dev_ref),
+    _append_acceleration_discovery(entries, _dev_ref)
-        lambda client: _publish_indicator_led_discovery(client, _dev_ref),
+    _append_indicator_led_discovery(entries, _dev_ref)
-        lambda client: _publish_backlight_status_discovery(client, _dev_ref),
+    _append_backlight_status_discovery(entries, _dev_ref)
-    )
+    _discovery_queue = entries
-    _discovery_phase_idx = 0
+    _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)
+            entries.append((old_t, b"", None))
-        _discovery_pause(client, count=2, delay_ms=15)
 
 
 def service_discovery():
-    global _discovery_phase_idx, _last_discovery_ms
+    global _discovery_idx, _last_discovery_ms
-    if client_ref is None or _discovery_phase_idx >= len(_discovery_phases):
+    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)
+    topic, payload, log_msg = _discovery_queue[_discovery_idx]
-    _discovery_phase_idx += 1
+    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):