361 lines
15 KiB
C++
361 lines
15 KiB
C++
#include "fgc/SerialMotorController.h"
|
|
|
|
#include "fgc/DumpParser.h"
|
|
#include "fgc/Logger.h"
|
|
#include "fgc/TelemetryParser.h"
|
|
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <cctype>
|
|
#include <deque>
|
|
#include <istream>
|
|
#include <mutex>
|
|
#include <thread>
|
|
|
|
#include <boost/asio.hpp>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
// A firmware register value is always "0x" + exactly 8 hex digits (printHex8).
|
|
bool isHex8(const std::string& v) {
|
|
if (v.size() != 10 || v[0] != '0' || (v[1] != 'x' && v[1] != 'X')) return false;
|
|
for (size_t i = 2; i < 10; ++i)
|
|
if (!std::isxdigit(static_cast<unsigned char>(v[i]))) return false;
|
|
return true;
|
|
}
|
|
|
|
// True only if a captured DUMP block decodes cleanly: each axis present must
|
|
// carry all 16 TMC registers with well-formed 8-hex values. The USB read is
|
|
// occasionally lossy, so this gates whether a block is trustworthy or must be
|
|
// re-requested (see captureDump).
|
|
bool dumpComplete(const std::string& block) {
|
|
static const char* kRegs[] = {
|
|
"GCONF", "GSTAT", "IOIN", "TSTEP", "RAMPMODE", "XACTUAL", "VACTUAL", "XTARGET",
|
|
"SW_MODE", "RAMP_STAT", "X_ENC", "ENC_STATUS", "CHOPCONF", "DRV_STATUS",
|
|
"PWM_SCALE", "PWM_AUTO"};
|
|
DumpData d = parseDump(block);
|
|
if (!d.valid || d.axes.empty()) return false;
|
|
for (const auto& ax : d.axes) {
|
|
for (const char* r : kRegs) {
|
|
auto it = ax.regs.find(r);
|
|
if (it == ax.regs.end() || !isHex8(it->second)) return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
struct SerialMotorController::Impl {
|
|
Impl(std::string dev, unsigned int b) : device(std::move(dev)), baud(b), serial(io) {}
|
|
|
|
std::string device;
|
|
unsigned int baud;
|
|
boost::asio::io_context io;
|
|
boost::asio::serial_port serial;
|
|
boost::asio::streambuf buffer;
|
|
std::thread io_thread;
|
|
std::mutex mutex;
|
|
MotorTelemetry latest;
|
|
std::atomic<bool> connected{false};
|
|
|
|
// DUMP capture: the firmware emits a "DUMP BEGIN" ... "DUMP END" block in
|
|
// response to a DUMP command. We accumulate it line-by-line and publish the
|
|
// completed block (latest_dump) for the UI / `dump` command. The USB read is
|
|
// occasionally lossy on the big dump burst, so an incomplete block is
|
|
// auto-re-requested up to dump_attempts times before we give up.
|
|
static constexpr int kMaxDumpAttempts = 8;
|
|
bool dumping = false;
|
|
int dump_attempts = 0; // remaining tries for the in-flight dump
|
|
std::string dump_buf;
|
|
std::string latest_dump;
|
|
|
|
// DIAG capture: the firmware streams "DG ..." lines (interleaved with ST)
|
|
// from "DG BEGIN" to "DG DONE". We accumulate the DG lines, log each one live
|
|
// (so it appears in the LOG pane), and publish the completed block.
|
|
bool diagging = false;
|
|
std::string diag_buf;
|
|
std::string latest_diag;
|
|
std::atomic<unsigned> diag_seq{0};
|
|
|
|
// NEED capture: single lines, queued for the control thread to answer once.
|
|
std::vector<std::string> need_lines;
|
|
|
|
// ENV: the latest reading of a DHT11 on the MCU and its fan (under mutex).
|
|
std::optional<McuEnvReading> latest_env;
|
|
|
|
// Lines waiting to go out, oldest first. Touched only on the io_context
|
|
// thread (every access is inside a posted handler or a completion), so it
|
|
// needs no lock.
|
|
std::deque<std::string> tx_queue;
|
|
// Lines handed to sendLine and not yet written, from any thread: stop()
|
|
// waits on it so a last command (the STOP on exit) is not dropped unsent.
|
|
std::atomic<int> tx_pending{0};
|
|
|
|
// Write one command line (newline-terminated) to the controller. Used by
|
|
// sendCommand and by the internal DUMP re-request.
|
|
//
|
|
// The write is initiated on the io_context thread (via post), NOT the
|
|
// caller's thread. asio I/O objects are not thread-safe for concurrent
|
|
// operations: issuing async_write from the main thread (e.g. the capture
|
|
// scheduler's MOVEs, or a TUI command) while the io_thread runs async_read
|
|
// on the same serial_port races the reader and corrupts inbound data — this
|
|
// was shredding the DUMP burst whenever the link was busy.
|
|
//
|
|
// And one line at a time: async_write is a series of write_some calls, and a
|
|
// second async_write started before the first completes may interleave its
|
|
// bytes with the first's. A burst - the 24 SETs of a drivetrain re-send -
|
|
// then reached the firmware as a spliced line ("SET Y SW_MODE 22SET P ..."),
|
|
// answered "ERR bad argument" and silently lost. So lines queue, and each
|
|
// write starts only when the previous one has finished.
|
|
void sendLine(const std::string& cmd) {
|
|
++tx_pending;
|
|
boost::asio::post(io, [this, line = cmd] {
|
|
LOG_TRACE_CAT(LogCat::Serial) << "TX " << line;
|
|
tx_queue.push_back(line + "\n");
|
|
if (tx_queue.size() == 1) writeNext(); // otherwise one is in flight
|
|
});
|
|
}
|
|
|
|
// Write the front of tx_queue; on completion drop it and go on. Runs on the
|
|
// io_context thread only.
|
|
void writeNext() {
|
|
boost::asio::async_write(serial, boost::asio::buffer(tx_queue.front()),
|
|
[this](const boost::system::error_code& ec, std::size_t) {
|
|
if (ec) LOG_WARN << "Serial write failed: " << ec.message();
|
|
tx_queue.pop_front();
|
|
--tx_pending;
|
|
if (!tx_queue.empty()) writeNext();
|
|
});
|
|
}
|
|
|
|
void doRead() {
|
|
boost::asio::async_read_until(
|
|
serial, buffer, '\n',
|
|
[this](const boost::system::error_code& ec, std::size_t) {
|
|
if (ec) {
|
|
LOG_WARN << "Serial read failed: " << ec.message();
|
|
return;
|
|
}
|
|
std::istream is(&buffer);
|
|
std::string line;
|
|
std::getline(is, line);
|
|
// Strip a trailing CR (firmware may send CRLF).
|
|
if (!line.empty() && line.back() == '\r') line.pop_back();
|
|
dispatchLine(line);
|
|
doRead();
|
|
});
|
|
}
|
|
|
|
// Route an inbound line: ST -> telemetry; OK/ERR -> command ack; everything
|
|
// else (DG/DUMP/BOOT/IOIN/GSTAT...) is async output we just trace.
|
|
void dispatchLine(const std::string& line) {
|
|
if (line.rfind("ST ", 0) == 0 || line == "ST") {
|
|
// A status line means the firmware has finished any dump in progress.
|
|
// If we were still assembling one, its DUMP END was lost on the wire —
|
|
// finalize now so the block is validated and (if incomplete) retried,
|
|
// rather than hanging forever waiting for an END that never arrives.
|
|
if (dumping) finalizeDump();
|
|
if (auto t = parseTelemetryLine(line)) {
|
|
LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
latest = *t;
|
|
} else {
|
|
LOG_TRACE_CAT(LogCat::Serial) << "RX(unparsed ST) " << line;
|
|
}
|
|
} else if (line.rfind("ERR", 0) == 0) {
|
|
LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
|
|
LOG_WARN << "Motor controller: " << line;
|
|
} else if (!line.empty()) {
|
|
// OK acks and other async output (DG/DUMP/BOOT/NEED/...).
|
|
LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
|
|
captureDump(line);
|
|
captureDiag(line);
|
|
captureNeed(line);
|
|
captureEnv(line);
|
|
}
|
|
}
|
|
|
|
// Keep the latest ENV line: one every 10 s from a tower with a DHT11 on the
|
|
// MCU, never from one without.
|
|
void captureEnv(const std::string& line) {
|
|
auto r = parseEnvLine(line);
|
|
if (!r) return;
|
|
r->epoch_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
std::chrono::system_clock::now().time_since_epoch()).count();
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
latest_env = *r;
|
|
}
|
|
|
|
// Queue a NEED line for the control thread to answer. Unlike DUMP and DIAG
|
|
// this is not a block to assemble - each line stands alone and is actionable
|
|
// immediately, so it is queued rather than accumulated, and taking it clears
|
|
// it. A firmware that re-asks (after a RESET, say) simply queues again.
|
|
void captureNeed(const std::string& line) {
|
|
if (line.rfind("NEED ", 0) != 0) return;
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
need_lines.push_back(line);
|
|
}
|
|
|
|
// Assemble the DIAG stream. DG lines arrive interleaved with ST over many
|
|
// seconds, so we accumulate only "DG " lines and log each live; on "DG DONE"
|
|
// we publish the block and bump diag_seq so the app can pick up the result.
|
|
void captureDiag(const std::string& line) {
|
|
const bool begin = line.find("DG BEGIN") != std::string::npos;
|
|
// Start on the FIRST BEGIN only; a per-axis "DG BEGIN P" mid-run must not
|
|
// wipe the already-captured first axis.
|
|
if (begin && !diagging) { diagging = true; diag_buf.clear(); }
|
|
if (!diagging) return;
|
|
if (line.rfind("DG ", 0) != 0 && !begin) return; // ignore non-DG lines
|
|
diag_buf += line + "\n";
|
|
LOG_INFO << line; // stream to the LOG pane
|
|
if (line.find("DG DONE") != std::string::npos) {
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
latest_diag = diag_buf;
|
|
}
|
|
++diag_seq;
|
|
diagging = false;
|
|
diag_buf.clear();
|
|
}
|
|
}
|
|
|
|
// Assemble the multi-line DUMP block. Detection of BEGIN/END must tolerate
|
|
// corruption: on a lossy read a dropped newline merges "DUMP END" onto the
|
|
// tail of the previous line (e.g. "...PWM_AUTO=0x...DUMP END"), so we search
|
|
// for the markers anywhere in the line, not just at the start. A merged/short
|
|
// block then fails dumpComplete() and is re-requested (see finalizeDump).
|
|
void captureDump(const std::string& line) {
|
|
size_t begin = line.find("DUMP BEGIN");
|
|
if (begin != std::string::npos) {
|
|
dumping = true;
|
|
dump_buf = line.substr(begin) + "\n"; // drop any junk before BEGIN
|
|
if (line.find("DUMP END", begin) != std::string::npos) finalizeDump();
|
|
return;
|
|
}
|
|
if (!dumping) return;
|
|
dump_buf += line + "\n";
|
|
if (line.find("DUMP END") != std::string::npos) finalizeDump();
|
|
}
|
|
|
|
void finalizeDump() {
|
|
dumping = false;
|
|
if (!dumpComplete(dump_buf) && dump_attempts > 1) {
|
|
// Lossy read — re-request rather than publish a corrupt block.
|
|
--dump_attempts;
|
|
LOG_WARN << "firmware dump incomplete (lossy read); re-requesting ("
|
|
<< dump_attempts << " attempt(s) left)";
|
|
sendLine("DUMP");
|
|
} else {
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex);
|
|
latest_dump = dump_buf;
|
|
}
|
|
if (dumpComplete(dump_buf)) LOG_INFO << "firmware dump:\n" << dump_buf;
|
|
else LOG_WARN << "firmware dump still incomplete after retries; showing best effort:\n"
|
|
<< dump_buf;
|
|
dump_attempts = 0;
|
|
}
|
|
dump_buf.clear();
|
|
}
|
|
};
|
|
|
|
SerialMotorController::SerialMotorController(std::string device, unsigned int baud)
|
|
: impl_(std::make_unique<Impl>(std::move(device), baud)) {}
|
|
|
|
SerialMotorController::~SerialMotorController() { stop(); }
|
|
|
|
void SerialMotorController::start() {
|
|
namespace asio = boost::asio;
|
|
boost::system::error_code ec;
|
|
impl_->serial.open(impl_->device, ec);
|
|
if (ec) {
|
|
LOG_ERROR << "Failed to open serial port " << impl_->device << ": " << ec.message();
|
|
impl_->connected = false;
|
|
return;
|
|
}
|
|
impl_->serial.set_option(asio::serial_port_base::baud_rate(impl_->baud));
|
|
impl_->serial.set_option(asio::serial_port_base::character_size(8));
|
|
impl_->serial.set_option(
|
|
asio::serial_port_base::parity(asio::serial_port_base::parity::none));
|
|
impl_->serial.set_option(
|
|
asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
|
|
impl_->serial.set_option(
|
|
asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));
|
|
|
|
impl_->connected = true;
|
|
impl_->doRead();
|
|
impl_->io_thread = std::thread([this] { impl_->io.run(); });
|
|
LOG_INFO << "Serial controller started on " << impl_->device << " @ " << impl_->baud;
|
|
}
|
|
|
|
void SerialMotorController::stop() {
|
|
if (!impl_) return;
|
|
// Let what is queued go out first (the STOP sent on exit, above all), but
|
|
// never hang shutdown on a link that has stopped accepting writes.
|
|
if (impl_->connected) {
|
|
const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500);
|
|
while (impl_->tx_pending > 0 && std::chrono::steady_clock::now() < deadline)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
|
}
|
|
impl_->io.stop();
|
|
if (impl_->io_thread.joinable()) impl_->io_thread.join();
|
|
boost::system::error_code ec;
|
|
if (impl_->serial.is_open()) impl_->serial.close(ec);
|
|
impl_->connected = false;
|
|
}
|
|
|
|
// Case-insensitive check for a bare "DUMP" command (optionally trailing ws).
|
|
static bool isDumpCommand(const std::string& cmd) {
|
|
size_t i = 0, n = cmd.size();
|
|
while (n > 0 && std::isspace(static_cast<unsigned char>(cmd[n - 1]))) --n;
|
|
while (i < n && std::isspace(static_cast<unsigned char>(cmd[i]))) ++i;
|
|
std::string t = cmd.substr(i, n - i);
|
|
if (t.size() != 4) return false;
|
|
return (t[0] == 'D' || t[0] == 'd') && (t[1] == 'U' || t[1] == 'u') &&
|
|
(t[2] == 'M' || t[2] == 'm') && (t[3] == 'P' || t[3] == 'p');
|
|
}
|
|
|
|
void SerialMotorController::sendCommand(const std::string& cmd) {
|
|
if (!impl_->connected) return;
|
|
// Arm dump-integrity retries when the operator requests a dump (the internal
|
|
// re-request goes through Impl::sendLine, which does not re-arm).
|
|
if (isDumpCommand(cmd)) impl_->dump_attempts = Impl::kMaxDumpAttempts;
|
|
impl_->sendLine(cmd);
|
|
}
|
|
|
|
MotorTelemetry SerialMotorController::telemetry() {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
return impl_->latest;
|
|
}
|
|
|
|
std::string SerialMotorController::lastDump() {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
return impl_->latest_dump;
|
|
}
|
|
|
|
std::string SerialMotorController::lastDiag() {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
return impl_->latest_diag;
|
|
}
|
|
|
|
unsigned SerialMotorController::diagSeq() const { return impl_->diag_seq.load(); }
|
|
|
|
std::optional<McuEnvReading> SerialMotorController::latestEnv() const {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
return impl_->latest_env;
|
|
}
|
|
|
|
std::vector<std::string> SerialMotorController::takeNeedLines() {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
std::vector<std::string> out;
|
|
out.swap(impl_->need_lines);
|
|
return out;
|
|
}
|
|
|
|
bool SerialMotorController::connected() const { return impl_->connected; }
|
|
|
|
} // namespace fgc
|