fix 🐛: SCSerial::wFlushSCS() is empty, causing TX echo to corrupt FeedBack() reads on half-duplex servo bus

[Yeah-Nah]

Summary

SCSerial::wFlushSCS() in SCServo/SCSerial.cpp has an empty body. On hardware where TX and RX share the same physical wire (half-duplex), this causes every call to FeedBack() to fail with -1, making it impossible to read live position, speed, load, or any other state from a bus servo.

---

Hardware

• Robot: Waveshare UGV Rover (pan-tilt gimbal variant)
• Servo: ST3215 Serial Bus Servo, IDs 1 (tilt) and 2 (pan)
• MCU: ESP32, firmware from waveshareteam/ugv_base_general
• Bus wiring (from General_Driver/ugv_config.h):

  #define S_RXD 18
  #define S_TXD 19

  GPIO 18 and GPIO 19 are both connected to the same single-wire half-duplex servo bus. Every byte transmitted on GPIO 19 is immediately echoed back on GPIO 18.

---

Observed symptom

The pan servo physically moves correctly when commanded via T=133 — writes work. However, the T=1001 telemetry response always reports a stuck pan value of -179.9560394 regardless of the servo's actual position.

With InfoPrint enabled (T=605 cmd=1), the firmware emits a continuous stream of T=1005 errors:

{"T":1005,"id":2,"status":0}
{"T":1005,"id":1,"status":0}

This confirms FeedBack() is returning -1 on every single call. Because getGimbalFeedback() only updates gimbalFeedback[].pos inside the if(st.FeedBack(...) != -1) guard, the position field stays at its zero-initialised C++ default (0), and panAngleCompute(0) computes:

$$\frac{(0 - 2047) \times 360}{4095} = -179.9560\ldots$$

---

Root cause

SCSerial::wFlushSCS() in SCServo/SCSerial.cpp is completely empty:

void SCSerial::wFlushSCS()
{
    // empty — does nothing
}

This function is called after every outgoing packet, before reading begins. Its purpose is to:

1. Block until the UART hardware has fully shifted out all queued bytes.
2. Drain the echo of those transmitted bytes from the RX buffer.

Because it does neither, the call chain for every FeedBack() read proceeds as follows:

rFlushSCS()   → clears stale RX bytes (correct)
writeBuf()    → queues request packet into TX FIFO, returns immediately
wFlushSCS()   → does nothing  ← BUG
checkHead()   → reads from RX looking for 0xFF 0xFF

By the time checkHead() executes, the UART is still transmitting. The TX echo lands in the RX buffer first. Since every SCServo request frame begins with 0xFF 0xFF, checkHead() picks up those two bytes from the echo, treats them as a servo response header, and then attempts to parse the remainder — which is the rest of the outgoing request, not a servo reply. The checksum fails, Read() returns 0, and FeedBack() returns -1. The servo's genuine response arrives moments later but is never read.

Writes work correctly because syncWrite() / SyncWritePosEx() only transmit and never attempt to read a response.

---

Fix

Two lines in SCServo/SCSerial.cpp:

// BEFORE
void SCSerial::wFlushSCS()
{
}

// AFTER
void SCSerial::wFlushSCS()
{
    pSerial->flush();  // block until TX FIFO fully drains
    rFlushSCS();       // discard the echoed TX bytes from the RX buffer
}

pSerial->flush() on Arduino/ESP32 blocks until the hardware UART has clocked out every queued byte. rFlushSCS() already exists in the class (while(pSerial->read() != -1) {}) and drains whatever is sitting in the RX buffer. After both calls the RX buffer is clean and the servo's response can be received correctly.

No other files need to be changed.

---

Notes

• rFlushSCS() is correctly implemented and does its job — the gap is only in wFlushSCS().
• The bug affects any read operation (FeedBack(), ReadPos(), ReadSpeed(), etc.) on any hardware where the TX pin echoes onto the RX pin, which is the standard wiring for a single-wire half-duplex bus.
• Write-only operations (SyncWritePosEx, WritePosEx, etc.) are unaffected.

[Copilot]

Pull request overview

This PR updates the vendored SCServo serial transport so read-style servo operations can work on half-duplex buses where transmitted bytes are echoed back into RX. In the context of this codebase, that supports reliable gimbal/servo telemetry on the ESP32-based robot firmware.

Changes:

• Implement SCSerial::wFlushSCS() so it waits for queued UART bytes to finish transmitting.
• Clear RX immediately after transmit to discard echoed request bytes before response parsing begins.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.