287 lines
12 KiB
C++
287 lines
12 KiB
C++
#include "fgc/SerialMotorController.h"
|
|
|
|
#include "fgc/DumpParser.h"
|
|
#include "fgc/Logger.h"
|
|
#include "fgc/TelemetryParser.h"
|
|
|
|
#include <atomic>
|
|
#include <cctype>
|
|
#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};
|
|
|
|
// 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.
|
|
void sendLine(const std::string& cmd) {
|
|
auto data = std::make_shared<std::string>(cmd + "\n");
|
|
auto trace = std::make_shared<std::string>(cmd);
|
|
boost::asio::post(io, [this, data, trace] {
|
|
LOG_TRACE_CAT(LogCat::Serial) << "TX " << *trace;
|
|
boost::asio::async_write(serial, boost::asio::buffer(*data),
|
|
[data](const boost::system::error_code& ec, std::size_t) {
|
|
if (ec) LOG_WARN << "Serial write failed: " << ec.message();
|
|
});
|
|
});
|
|
}
|
|
|
|
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/...).
|
|
LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
|
|
captureDump(line);
|
|
captureDiag(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;
|
|
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(); }
|
|
|
|
bool SerialMotorController::connected() const { return impl_->connected; }
|
|
|
|
} // namespace fgc
|