364 lines
20 KiB
C++
364 lines
20 KiB
C++
// SPDX-License-Identifier: GPL-3.0-or-later
|
|
// Private per-device JSON process adapter. Every wire operation is upstream.
|
|
#include "config_export.h"
|
|
#include <QEventLoop>
|
|
#include <QElapsedTimer>
|
|
#include <QSocketNotifier>
|
|
#include <QSerialPort>
|
|
#include <QTimer>
|
|
#include <QFileInfo>
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <fcntl.h>
|
|
#include <sys/file.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/sysmacros.h>
|
|
#include <termios.h>
|
|
#include <unistd.h>
|
|
|
|
class ExchangeFailure : public std::runtime_error {
|
|
public:
|
|
QJsonObject diagnostics;
|
|
explicit ExchangeFailure(const QJsonObject &value)
|
|
: std::runtime_error("Native query timed out or disconnected"), diagnostics(value) {}
|
|
};
|
|
|
|
class Engine {
|
|
public:
|
|
VescInterface vesc;
|
|
Packet *packet;
|
|
FW_RX_PARAMS identity;
|
|
bool procedureRunning = false;
|
|
bool procedureUncertain = false;
|
|
QJsonObject procedure;
|
|
bool queryRunning = false;
|
|
bool allowHardware = false;
|
|
QByteArray lastMotor, lastApplication, lastHall;
|
|
|
|
Engine() {
|
|
require(Utility::configLoadLatest(&vesc), "Upstream resources missing");
|
|
packet = vesc.findChild<Packet *>();
|
|
require(packet, "Upstream packet transport missing");
|
|
QObject::connect(vesc.commands(), &Commands::fwVersionReceived,
|
|
[&](FW_RX_PARAMS value) { identity = value; });
|
|
QObject::connect(packet, &Packet::packetReceived, [&](QByteArray &raw) {
|
|
if (!raw.isEmpty() && quint8(raw[0]) == COMM_GET_MCCONF) lastMotor = raw;
|
|
if (!raw.isEmpty() && quint8(raw[0]) == COMM_GET_APPCONF) lastApplication = raw;
|
|
if (!raw.isEmpty() && quint8(raw[0]) == COMM_DETECT_HALL_FOC) lastHall = raw;
|
|
});
|
|
}
|
|
|
|
void open(const QString &port) {
|
|
require(QRegularExpression("^/dev/ttyACM[0-9]+$").match(port).hasMatch(), "Unsupported USB path");
|
|
struct stat st;
|
|
require(lstat(port.toLocal8Bit(), &st) == 0 && S_ISCHR(st.st_mode) && major(st.st_rdev) == 166,
|
|
"Not a CDC ACM device");
|
|
require(vesc.connectSerial(port, 115200), "Native serial connection failed");
|
|
auto serial = vesc.findChild<QSerialPort *>();
|
|
require(serial && serial->isOpen() && "/dev/" + serial->portName() == port,
|
|
"Native serial path mismatch");
|
|
require(flock(serial->handle(), LOCK_EX | LOCK_NB) == 0 && ioctl(serial->handle(), TIOCEXCL) == 0,
|
|
"Serial port is already owned");
|
|
allowHardware = true;
|
|
query(COMM_FW_VERSION, 3000);
|
|
require(identity.major == 5 && identity.minor == 2 && identity.hwType == HW_TYPE_VESC
|
|
&& identity.isTestFw == 0 && identity.customConfigNum == 0,
|
|
"Native hardware acceptance currently admits stable FW 5.02 only");
|
|
}
|
|
|
|
QByteArray exchange(int command, int timeoutMs, const std::function<void()> &send) {
|
|
require(allowHardware && vesc.isPortConnected(), "Device disconnected");
|
|
require(!queryRunning, "A native query is already pending");
|
|
queryRunning = true;
|
|
struct PendingReset { bool &pending; ~PendingReset() { pending = false; } } reset{queryRunning};
|
|
QEventLoop loop;
|
|
QTimer timeout; timeout.setSingleShot(true);
|
|
QObject observer;
|
|
QElapsedTimer elapsed; elapsed.start();
|
|
QJsonArray events;
|
|
bool emitted = false;
|
|
int packetsSent = 0, packetsReceived = 0;
|
|
qint64 bytesWritten = 0;
|
|
auto serial = vesc.findChild<QSerialPort *>();
|
|
auto record = [&](const QString &kind, int code, qint64 bytes) {
|
|
if (events.size() == 24) events.removeFirst();
|
|
events.append(QJsonObject{{"event", kind}, {"command", code},
|
|
{"bytes", double(bytes)}, {"at_ms", elapsed.nsecsElapsed() / 1e6}});
|
|
};
|
|
QObject::connect(vesc.commands(), &Commands::dataToSend, &observer, [&](QByteArray &raw) {
|
|
const int code = raw.isEmpty() ? -1 : quint8(raw[0]);
|
|
if (code == command) emitted = true;
|
|
record("command_emitted", code, raw.size());
|
|
});
|
|
QObject::connect(packet, &Packet::dataToSend, &observer, [&](QByteArray &raw) {
|
|
++packetsSent; record("packet_sent", -1, raw.size());
|
|
});
|
|
if (serial) {
|
|
QObject::connect(serial, &QSerialPort::bytesWritten, &observer, [&](qint64 bytes) {
|
|
bytesWritten += bytes; record("serial_written", -1, bytes);
|
|
});
|
|
QObject::connect(serial, &QSerialPort::errorOccurred, &observer, [&](QSerialPort::SerialPortError error) {
|
|
if (error != QSerialPort::NoError) record("serial_error", int(error), 0);
|
|
});
|
|
}
|
|
QByteArray answer;
|
|
QObject::connect(packet, &Packet::packetReceived, &observer, [&](QByteArray &raw) {
|
|
++packetsReceived;
|
|
record("packet_received", raw.isEmpty() ? -1 : quint8(raw[0]), raw.size());
|
|
if (!raw.isEmpty() && quint8(raw[0]) == command) { answer = raw; loop.quit(); }
|
|
});
|
|
QObject::connect(&timeout, &QTimer::timeout, &loop, &QEventLoop::quit);
|
|
timeout.start(timeoutMs);
|
|
send();
|
|
if (answer.isEmpty()) loop.exec();
|
|
if (answer.isEmpty() || !vesc.isPortConnected()) {
|
|
throw ExchangeFailure({{"requested_command", command}, {"timeout_ms", timeoutMs},
|
|
{"elapsed_ms", elapsed.nsecsElapsed() / 1e6}, {"request_emitted", emitted},
|
|
{"packets_sent", packetsSent}, {"packets_received", packetsReceived},
|
|
{"serial_bytes_written", double(bytesWritten)}, {"port_connected", vesc.isPortConnected()},
|
|
{"serial_open", serial && serial->isOpen()}, {"serial_error", serial ? int(serial->error()) : -1},
|
|
{"serial_bytes_pending", serial ? double(serial->bytesToWrite()) : -1}, {"events", events}});
|
|
}
|
|
return answer;
|
|
}
|
|
|
|
QByteArray query(int code, int timeoutMs, bool internal = false) {
|
|
auto cmd = vesc.commands();
|
|
require(internal || !procedureRunning || code == COMM_GET_VALUES || code == COMM_GET_DECODED_PPM,
|
|
"Configuration reads are unavailable during native measurement");
|
|
std::function<void()> send;
|
|
switch (code) {
|
|
case COMM_FW_VERSION: send = [=] { cmd->getFwVersion(); }; break;
|
|
case COMM_GET_VALUES: send = [=] { cmd->getValues(); }; break;
|
|
case COMM_GET_MCCONF: send = [=] { cmd->getMcconf(); }; break;
|
|
case COMM_GET_APPCONF: send = [=] { cmd->getAppConf(); }; break;
|
|
case COMM_GET_DECODED_PPM: send = [=] { cmd->getDecodedPpm(); }; break;
|
|
case COMM_PING_CAN: send = [=] { cmd->pingCan(); }; break;
|
|
default: throw std::runtime_error("Native read command is not admitted");
|
|
}
|
|
auto raw = exchange(code, timeoutMs, send);
|
|
if (code == COMM_GET_MCCONF || code == COMM_GET_APPCONF) {
|
|
VByteArray serialized;
|
|
auto config = code == COMM_GET_MCCONF ? vesc.mcConfig() : vesc.appConfig();
|
|
config->serialize(serialized);
|
|
require(serialized == raw.mid(1), "Native configuration decode is not byte-exact");
|
|
}
|
|
return raw;
|
|
}
|
|
|
|
QByteArray configurationPacket(ConfigParams *config, int code) {
|
|
VByteArray raw; raw.vbAppendInt8(code); config->serialize(raw); return raw;
|
|
}
|
|
|
|
void calibrate(double loss) {
|
|
ConfigParams beforeMotor, beforeApp;
|
|
beforeMotor = *vesc.mcConfig(); beforeApp = *vesc.appConfig();
|
|
bool received = false, validated = false;
|
|
int code = -1000;
|
|
QString report, error;
|
|
QJsonArray changed;
|
|
auto connection = QObject::connect(vesc.commands(), &Commands::detectAllFocReceived,
|
|
[&](int result) { received = true; code = result; });
|
|
try {
|
|
// The actual upstream wizard motor procedure, including its FW 5.02
|
|
// power-loss correction. Do not infer a battery profile from Ah/voltage.
|
|
report = Utility::detectAllFoc(&vesc, false, loss,
|
|
beforeMotor.getParamDouble("l_in_current_min"), beforeMotor.getParamDouble("l_in_current_max"),
|
|
beforeMotor.getParamDouble("foc_openloop_rpm"), beforeMotor.getParamDouble("foc_sl_erpm"));
|
|
require(received, "Native calibration completion was not received");
|
|
query(COMM_GET_MCCONF, 3000, true);
|
|
query(COMM_GET_APPCONF, 3000, true);
|
|
auto mc = vesc.mcConfig(); auto app = vesc.appConfig();
|
|
const QStringList admitted = {"l_current_max", "l_current_min", "motor_type", "foc_motor_r",
|
|
"foc_motor_l", "foc_motor_flux_linkage", "foc_current_kp", "foc_current_ki", "foc_observer_gain",
|
|
"foc_sensor_mode", "m_sensor_port_mode", "foc_encoder_offset", "foc_encoder_ratio", "foc_encoder_inverted",
|
|
"foc_hall_table__0", "foc_hall_table__1", "foc_hall_table__2", "foc_hall_table__3",
|
|
"foc_hall_table__4", "foc_hall_table__5", "foc_hall_table__6", "foc_hall_table__7"};
|
|
for (const auto &key : beforeMotor.checkDifference(mc)) {
|
|
require(admitted.contains(key), "Calibration changed a protected motor parameter");
|
|
changed.append(key);
|
|
}
|
|
for (const auto &key : beforeApp.checkDifference(app))
|
|
require(key == "send_can_status", "Calibration changed a protected receiver parameter");
|
|
// Native FW 5.02 enables CAN status as a side effect. Restore the
|
|
// original application exactly; retain existing PPM and CAN identity.
|
|
if (!beforeApp.checkDifference(app).isEmpty()) {
|
|
*app = beforeApp;
|
|
const auto expected = configurationPacket(app, COMM_GET_APPCONF);
|
|
require(exchange(COMM_SET_APPCONF, 3000, [&] { vesc.commands()->setAppConf(); }).size() == 1,
|
|
"Application write ACK invalid");
|
|
require(query(COMM_GET_APPCONF, 3000, true) == expected, "Application restore not byte-exact");
|
|
}
|
|
if (code < 0) {
|
|
// A completed failed detection can leave partial RAM changes.
|
|
// Only the known calibration fields passed the guard above.
|
|
*mc = beforeMotor;
|
|
} else {
|
|
for (const auto &key : {"foc_motor_r", "foc_motor_l", "foc_motor_flux_linkage"})
|
|
require(std::isfinite(mc->getParamDouble(key)) && mc->getParamDouble(key) > 0,
|
|
"Invalid detected motor parameter");
|
|
require(mc->getParamDouble("l_current_max") > 0 && mc->getParamDouble("l_current_min") < 0,
|
|
"Invalid detected current limits");
|
|
// Calibration must not silently increase the owner's existing
|
|
// current limits. The separate spin test applies its own 30 A cap.
|
|
mc->updateParamDouble("l_current_max", qMin(mc->getParamDouble("l_current_max"), beforeMotor.getParamDouble("l_current_max")));
|
|
mc->updateParamDouble("l_current_min", qMax(mc->getParamDouble("l_current_min"), beforeMotor.getParamDouble("l_current_min")));
|
|
}
|
|
const auto expected = configurationPacket(mc, COMM_GET_MCCONF);
|
|
if (expected != lastMotor) {
|
|
require(exchange(COMM_SET_MCCONF, 3000, [&] { vesc.commands()->setMcconf(false); }).size() == 1,
|
|
"Motor write ACK invalid");
|
|
require(query(COMM_GET_MCCONF, 3000, true) == expected, "Motor write not byte-exact");
|
|
}
|
|
validated = true;
|
|
} catch (const std::exception &e) { error = e.what(); }
|
|
QObject::disconnect(connection);
|
|
QJsonObject parameters;
|
|
for (const auto &key : {"l_current_max", "l_current_min", "foc_motor_r", "foc_motor_l", "foc_motor_flux_linkage"})
|
|
parameters.insert(key, vesc.mcConfig()->getParamDouble(key));
|
|
procedure = {{"kind", "foc"}, {"completed", received}, {"success", received && code >= 0 && validated},
|
|
{"validated", validated}, {"code", code}, {"report", report}, {"error", error},
|
|
{"sensor_mode", vesc.mcConfig()->getParamEnum("foc_sensor_mode")}, {"parameters", parameters},
|
|
{"changed", changed}, {"upstream", "Utility::detectAllFoc"}};
|
|
procedureUncertain = !validated;
|
|
procedureRunning = false;
|
|
}
|
|
|
|
double number(const QJsonObject &request, const QString &name, double min, double max) {
|
|
auto v = request.value(name);
|
|
require(v.isDouble() && std::isfinite(v.toDouble()) && v.toDouble() >= min && v.toDouble() <= max,
|
|
"Numeric argument is outside operation bounds");
|
|
return v.toDouble();
|
|
}
|
|
|
|
void flush() {
|
|
auto serial = vesc.findChild<QSerialPort *>();
|
|
require(serial && serial->isOpen(), "Serial transport closed");
|
|
serial->flush();
|
|
if (serial->bytesToWrite() > 0) require(serial->waitForBytesWritten(40), "Serial write not confirmed");
|
|
require(serial->bytesToWrite() == 0, "Serial write is incomplete");
|
|
}
|
|
|
|
QJsonObject dispatch(const QJsonObject &request) {
|
|
const auto method = request.value("method").toString();
|
|
if (method == "engine") return {{"version", "7.00"}, {"commit", "01d5f10901116c311e3fb84d5a1541f663d3ce20"},
|
|
{"connected", vesc.isPortConnected()}, {"hardware_enabled", allowHardware},
|
|
{"legacy_power_loss_correction", vesc.commands()->getMaxPowerLossBug()}};
|
|
require(allowHardware && vesc.isPortConnected(), "Native device is not connected");
|
|
if (method == "query") return {{"payload", QString::fromLatin1(query(
|
|
int(number(request, "command", 0, 255)), int(number(request, "timeout_ms", 20, 8000))).toBase64())}};
|
|
if (method == "procedure_result") return {{"running", procedureRunning}, {"uncertain", procedureUncertain}, {"result", procedure}};
|
|
if (method == "lease") { vesc.commands()->disableAppOutput(250, false); flush(); return {}; }
|
|
if (method == "release") { vesc.commands()->setCurrent(0); flush(); return {}; }
|
|
require(!procedureRunning && !procedureUncertain, "Native procedure owns this controller");
|
|
if (method == "current") {
|
|
auto current = number(request, "current_a", 0, 30);
|
|
require(current <= vesc.mcConfig()->getParamDouble("l_current_max"), "Configured current limit exceeded");
|
|
vesc.commands()->setCurrent(current); flush(); return {};
|
|
}
|
|
if (method == "rpm") { vesc.commands()->setRpm(int(number(request, "erpm", 0, 3000))); flush(); return {}; }
|
|
if (method == "limits") {
|
|
auto p = request.value("parameters").toObject();
|
|
MCCONF_TEMP conf;
|
|
conf.current_min_scale = number(p, "l_current_min_scale", 0, 1);
|
|
conf.current_max_scale = number(p, "l_current_max_scale", 0, 1);
|
|
// Restore/application may only change current scales. All other
|
|
// values must equal the last native read, including battery limits.
|
|
auto mc = vesc.mcConfig();
|
|
for (const auto &key : {"l_min_erpm", "l_max_erpm", "l_min_duty", "l_max_duty", "l_watt_min", "l_watt_max", "l_in_current_min", "l_in_current_max"})
|
|
require(p.value(key).isDouble() && p.value(key).toDouble() == mc->getParamDouble(key), "Only volatile current scales may change");
|
|
conf.erpm_or_speed_min = mc->getParamDouble("l_min_erpm");
|
|
conf.erpm_or_speed_max = mc->getParamDouble("l_max_erpm");
|
|
conf.duty_min = mc->getParamDouble("l_min_duty"); conf.duty_max = mc->getParamDouble("l_max_duty");
|
|
conf.watt_min = mc->getParamDouble("l_watt_min"); conf.watt_max = mc->getParamDouble("l_watt_max");
|
|
auto ack = exchange(COMM_SET_MCCONF_TEMP, 2000, [&] {
|
|
vesc.commands()->setMcconfTemp(conf, false, false, false, false, true);
|
|
});
|
|
require(ack.size() == 1, "Invalid native limits ACK"); return {};
|
|
}
|
|
if (method == "configuration") {
|
|
auto motor = query(COMM_GET_MCCONF, 2000);
|
|
auto application = query(COMM_GET_APPCONF, 2000);
|
|
return {{"motor", exportConfig(vesc.mcConfig(), motor, "MCConfiguration")},
|
|
{"application", exportConfig(vesc.appConfig(), application, "APPConfiguration")}};
|
|
}
|
|
if (method == "foc_start") {
|
|
const auto loss = number(request, "max_power_loss_w", 10, 150);
|
|
require(!lastMotor.isEmpty() && !lastApplication.isEmpty(), "Read configurations before calibration");
|
|
for (const auto &key : {"l_in_current_min", "l_in_current_max", "foc_openloop_rpm", "foc_sl_erpm"})
|
|
require(std::isfinite(vesc.mcConfig()->getParamDouble(key)) && std::abs(vesc.mcConfig()->getParamDouble(key)) > 0.001,
|
|
"Zero-valued detection inputs require an explicit equipment profile");
|
|
procedureRunning = true; procedure = {{"kind", "foc"}};
|
|
QTimer::singleShot(0, [this, loss] { calibrate(loss); });
|
|
return {{"started", true}, {"interruptible", false}};
|
|
}
|
|
if (method == "hall_start") {
|
|
require(request.value("current_a") == 5, "This Hall profile uses 5 A");
|
|
procedureRunning = true; procedure = {}; lastHall.clear();
|
|
QTimer::singleShot(0, [&] {
|
|
auto measured = Utility::measureHallFocBlocking(&vesc, 5.0);
|
|
QJsonArray table;
|
|
for (int i = 1; i < measured.size(); ++i) table.append(measured[i]);
|
|
const bool completed = measured.size() == 9 && measured.first() != -10;
|
|
procedure = {{"kind", "hall"}, {"completed", completed},
|
|
{"status", measured.isEmpty() ? -10 : measured.first()}, {"table", table},
|
|
{"payload", QString::fromLatin1(lastHall.toBase64())},
|
|
{"upstream", "Utility::measureHallFocBlocking"}};
|
|
procedureUncertain = !completed;
|
|
procedureRunning = false;
|
|
});
|
|
return {{"started", true}, {"interruptible", false}};
|
|
}
|
|
throw std::runtime_error("Native operation is not admitted");
|
|
}
|
|
};
|
|
|
|
int main(int argc, char **argv) {
|
|
qputenv("QT_QPA_PLATFORM", "offscreen");
|
|
QApplication application(argc, argv);
|
|
QCoreApplication::setOrganizationName("MissionCore");
|
|
QCoreApplication::setApplicationName("VescToolEngine");
|
|
try {
|
|
require(argc == 2, "One exact serial port or --offline argument is required");
|
|
Engine engine;
|
|
const QString port = QString::fromLocal8Bit(argv[1]);
|
|
if (port != "--offline") engine.open(port);
|
|
QFile output; output.open(stdout, QIODevice::WriteOnly);
|
|
auto write = [&](const QJsonObject &value) {
|
|
output.write(QJsonDocument(value).toJson(QJsonDocument::Compact) + '\n'); output.flush();
|
|
};
|
|
write({{"ready", true}, {"engine", engine.dispatch({{"method", "engine"}})}});
|
|
QByteArray buffer;
|
|
fcntl(STDIN_FILENO, F_SETFL, fcntl(STDIN_FILENO, F_GETFL) | O_NONBLOCK);
|
|
QSocketNotifier input(STDIN_FILENO, QSocketNotifier::Read);
|
|
QObject::connect(&input, &QSocketNotifier::activated, [&] {
|
|
char chunk[4096]; const auto size = ::read(STDIN_FILENO, chunk, sizeof(chunk));
|
|
if (size == 0) { application.quit(); return; }
|
|
if (size < 0) return;
|
|
buffer.append(chunk, int(size));
|
|
if (buffer.size() > 65536) { application.exit(2); return; }
|
|
int end;
|
|
while ((end = buffer.indexOf('\n')) >= 0) {
|
|
auto raw = buffer.left(end); buffer.remove(0, end + 1);
|
|
QJsonParseError error;
|
|
auto document = QJsonDocument::fromJson(raw, &error);
|
|
auto request = document.object();
|
|
QJsonObject response{{"id", request.value("id")}};
|
|
try {
|
|
require(error.error == QJsonParseError::NoError && document.isObject(), "Invalid JSON request");
|
|
response.insert("result", engine.dispatch(request)); response.insert("ok", true);
|
|
} catch (const ExchangeFailure &e) {
|
|
response.insert("ok", false); response.insert("error", e.what());
|
|
response.insert("diagnostics", e.diagnostics);
|
|
} catch (const std::exception &e) { response.insert("ok", false); response.insert("error", e.what()); }
|
|
write(response);
|
|
}
|
|
});
|
|
return application.exec();
|
|
} catch (const std::exception &error) {
|
|
fprintf(stderr, "Native engine startup failed: %s\n", error.what());
|
|
return 1;
|
|
}
|
|
}
|