124 lines
4.5 KiB
C++
124 lines
4.5 KiB
C++
#include "fgc/SerialMotorController.h"
|
|
|
|
#include "fgc/Logger.h"
|
|
#include "fgc/TelemetryParser.h"
|
|
|
|
#include <atomic>
|
|
#include <istream>
|
|
#include <mutex>
|
|
#include <thread>
|
|
|
|
#include <boost/asio.hpp>
|
|
|
|
namespace fgc {
|
|
|
|
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};
|
|
|
|
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") {
|
|
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;
|
|
}
|
|
}
|
|
};
|
|
|
|
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;
|
|
}
|
|
|
|
void SerialMotorController::sendCommand(const std::string& cmd) {
|
|
if (!impl_->connected) return;
|
|
LOG_TRACE_CAT(LogCat::Serial) << "TX " << cmd;
|
|
// The firmware requires a newline terminator on every command.
|
|
auto data = std::make_shared<std::string>(cmd + "\n");
|
|
boost::asio::async_write(impl_->serial, boost::asio::buffer(*data),
|
|
[data](const boost::system::error_code& ec, std::size_t) {
|
|
if (ec) LOG_WARN << "Serial write failed: " << ec.message();
|
|
});
|
|
}
|
|
|
|
MotorTelemetry SerialMotorController::telemetry() {
|
|
std::lock_guard<std::mutex> lock(impl_->mutex);
|
|
return impl_->latest;
|
|
}
|
|
|
|
bool SerialMotorController::connected() const { return impl_->connected; }
|
|
|
|
} // namespace fgc
|