Index: tools/CANDumpPlayer/main.cpp =================================================================== diff -u -rcfc0df719cb5033078d0cac45ce0f6243810f2e7 -r70e672866cdc9e334ed725b82bde6ba6574420bf --- tools/CANDumpPlayer/main.cpp (.../main.cpp) (revision cfc0df719cb5033078d0cac45ce0f6243810f2e7) +++ tools/CANDumpPlayer/main.cpp (.../main.cpp) (revision 70e672866cdc9e334ed725b82bde6ba6574420bf) @@ -1,96 +1,183 @@ +#include + #include -#include #include +#include +#include #include +#include #include +#include #include #include #include #include -#include int main(int argc, char *argv[]) { QCoreApplication app(argc, argv); - app.setApplicationName("CANDUmpPlayer"); + app.setApplicationName("CANDumpPlayer"); app.setApplicationVersion("1.0"); QCommandLineParser parser; + parser.setApplicationDescription("Replays a candump log file onto a CAN interface."); parser.addHelpOption(); parser.addVersionOption(); - parser.addPositionalArgument("can_interface", "CAN device"); - parser.addPositionalArgument("candump_file", "Input CAN dump file"); + parser.addPositionalArgument("can_interface", "CAN device (e.g. can0). Not required with --test."); + parser.addPositionalArgument("candump_file", "Input CAN dump file."); + + QCommandLineOption speedOption({"s", "speed"}, + "Replay speed multiplier (float). 0 = immediate (default), 1 = real-time, " + " = x faster than real-time.", + "speed", "0"); + parser.addOption(speedOption); + + QCommandLineOption testOption({"t", "test"}, + "Test mode: skip CAN interface, print each frame with calculated and actual " + "time deltas. Pass the candump file as the only positional argument."); + parser.addOption(testOption); + parser.process(app); - const QStringList args = parser.positionalArguments(); + const bool testMode = parser.isSet(testOption); - if (args.length() != 2) { + bool speedOk = false; + const double speed = parser.value(speedOption).toDouble(&speedOk); + if (!speedOk || speed < 0.0) { + qCritical().noquote() << "ERROR: --speed must be a non-negative floating point number."; + return 1; + } + + const QStringList args = parser.positionalArguments(); + const int expectedArgs = testMode ? 1 : 2; + if (args.length() != expectedArgs) { qCritical().noquote() << Qt::endl - << QString("ERROR: incorrect number of arguments (expected 2, but received %1)").arg(args.length()) << Qt::endl; + << QString("ERROR: incorrect number of arguments (expected %1, but received %2).") + .arg(expectedArgs) + .arg(args.length()) + << Qt::endl; parser.showHelp(1); return 1; } - QString error; - QSharedPointer can_device(QCanBus::instance()->createDevice(QStringLiteral("socketcan"), args.at(0), &error)); - if (can_device == nullptr) { - qCritical().noquote() << QString("ERROR: could not open CAN device %1 (error=%2)").arg(args.at(0)).arg(error); - return 1; + QSharedPointer canDevice; + if (!testMode) { + QString error; + canDevice.reset(QCanBus::instance()->createDevice(QStringLiteral("socketcan"), args.at(0), &error)); + if (!canDevice) { + qCritical().noquote() + << QString("ERROR: could not open CAN device %1 (error=%2)").arg(args.at(0)).arg(error); + return 1; + } + canDevice->setConfigurationParameter(QCanBusDevice::CanFdKey, false); + canDevice->setConfigurationParameter(QCanBusDevice::BitRateKey, 250000); + canDevice->connectDevice(); } - can_device->setConfigurationParameter(QCanBusDevice::CanFdKey, false); - can_device->setConfigurationParameter(QCanBusDevice::BitRateKey, 250000); - can_device->connectDevice(); - QFile can_file(args.at(1)); - if (can_file.open(QIODevice::ReadOnly | QIODevice::Text) == false) { - qCritical().noquote() << QString("ERROR: could not open input CAN dump file %1").arg(args.at(1)); + const QString canFile = testMode ? args.at(0) : args.at(1); + QFile file(canFile); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + qCritical().noquote() << QString("ERROR: could not open input CAN dump file %1").arg(canFile); return 1; } - QTextStream stream(&can_file); + QTextStream stream(&file); QString line; - const QRegularExpression regexEntry("^\\s*\\((?.+)\\)\\s+(?\\S+)\\s+(?\\S+)\\s+\\[(?\\S+)\\]\\s+(?.*)$"); + const QRegularExpression regexEntry( + "^\\s*\\((?.+)\\)\\s+(?\\S+)\\s+(?\\S+)\\s+\\[(?\\S+)\\]\\s+(?.*)$" + ); const QRegularExpression regexPayload("(?:\\s*)(\\S+)"); + + // Candump files may use either Unix epoch ("1234567890.123456") or + // wall-clock ("YYYY-MM-DD HH:MM:SS.ffffff") timestamp format. + // We parse both and normalise to microseconds since epoch as qint64. + const QString datetimeFormat = QStringLiteral("yyyy-MM-dd HH:mm:ss.zzz"); + + auto parseTimestampUs = [&](const QString &raw) -> qint64 { + // Try wall-clock format first (contains a space). + if (raw.contains(' ')) { + // QDateTime::fromString only handles millisecond precision with 'zzz'. + // Truncate the fractional part to 3 digits for parsing, then recover + // the full microseconds from the original string. + const int dotPos = raw.lastIndexOf('.'); + const QString msRaw = raw.left(dotPos + 4); // "YYYY-MM-DD HH:MM:SS.mmm" + const QDateTime dt = QDateTime::fromString(msRaw, datetimeFormat); + if (!dt.isValid()) { + return -1; + } + const qint64 subSecUs = raw.mid(dotPos + 1).leftJustified(6, '0').left(6).toLongLong(); + return dt.toMSecsSinceEpoch() / 1000 * 1'000'000 + subSecUs; + } + // Unix epoch format: parse as double then convert. + bool ok = false; + const double epochSec = raw.toDouble(&ok); + return ok ? static_cast(epochSec * 1'000'000) : -1; + }; + + // Record the monotonic clock origin at the start of replay. + // Each frame's absolute wake time is: origin + (logOffset / speed). + // This absorbs per-frame overshoot automatically — no drift accumulation. + struct timespec replayOrigin; + clock_gettime(CLOCK_MONOTONIC, &replayOrigin); + + qint64 firstTimestampUs = -1; + qint64 prevTimestampUs = -1; + QElapsedTimer wallTimer; unsigned int lineCount = 1; + while (stream.readLineInto(&line)) { auto match = regexEntry.match(line); if (match.hasMatch()) { - // qDebug().noquote() << " timestamp=" << match.captured(QStringLiteral("timestamp")); - // qDebug().noquote() << " device=" << match.captured(QStringLiteral("device")); - // qDebug().noquote() << " can_id=" << match.captured(QStringLiteral("can_id")); - // qDebug().noquote() << " size=" << match.captured(QStringLiteral("size")); - // qDebug().noquote() << " payload=" << match.captured(QStringLiteral("payload")); - QByteArray payload; auto it = regexPayload.globalMatch(match.captured(QStringLiteral("payload"))); while (it.hasNext()) { auto payloadMatch = it.next(); - payload.append(payloadMatch.captured(1).toUInt(nullptr, 16)); + payload.append(static_cast(payloadMatch.captured(1).toUInt(nullptr, 16))); } - const unsigned int can_id = match.captured(QStringLiteral("can_id")).toUInt(nullptr, 16); - QCanBusFrame frame(can_id, payload); - can_device->writeFrame(frame); + const qint64 timestampUs = parseTimestampUs(match.captured(QStringLiteral("timestamp"))); + const qint64 calcDeltaUs = (prevTimestampUs >= 0 && timestampUs >= 0) ? timestampUs - prevTimestampUs : 0; + + if (speed > 0.0 && timestampUs >= 0) { + if (firstTimestampUs < 0) { + firstTimestampUs = timestampUs; + } + const qint64 offsetUs = static_cast((timestampUs - firstTimestampUs) / speed); + struct timespec wakeTime; + wakeTime.tv_sec = replayOrigin.tv_sec + offsetUs / 1'000'000; + wakeTime.tv_nsec = replayOrigin.tv_nsec + (offsetUs % 1'000'000) * 1000; + wakeTime.tv_sec += wakeTime.tv_nsec / 1'000'000'000; + wakeTime.tv_nsec = wakeTime.tv_nsec % 1'000'000'000; + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeTime, nullptr); + } + + if (testMode) { + const qint64 actualDeltaUs = (prevTimestampUs >= 0) ? wallTimer.nsecsElapsed() / 1000 : 0; + wallTimer.restart(); + qInfo().noquote() << QString("delta(calc=%1us, actual=%2us) %3") + .arg(calcDeltaUs, 6) + .arg(actualDeltaUs, 6) + .arg(line.trimmed()); + } + else { + const unsigned int canId = match.captured(QStringLiteral("can_id")).toUInt(nullptr, 16); + QCanBusFrame frame(canId, payload); + canDevice->writeFrame(frame); + } + + prevTimestampUs = timestampUs; } else { - qWarning().noquote() << QString("WARNING: \"%1\" (line %2) did not match expected format").arg(line).arg(lineCount); + qWarning().noquote() + << QString("WARNING: \"%1\" (line %2) did not match expected format").arg(line).arg(lineCount); } lineCount++; } - can_file.close(); - can_device->disconnectDevice(); + file.close(); + if (canDevice) { + canDevice->disconnectDevice(); + } - // Set up code that uses the Qt event loop here. - // Call a.quit() or a.exit() to quit the application. - // A not very useful example would be including - // #include - // near the top of the file and calling - // QTimer::singleShot(5000, &a, &QCoreApplication::quit); - // which quits the application after 5 seconds. - - // If you do not need a running Qt event loop, remove the call - // to a.exec() or use the Non-Qt Plain C++ Application template. - return 0; } Index: tools/CMakeLists.txt =================================================================== diff -u -rcfc0df719cb5033078d0cac45ce0f6243810f2e7 -r70e672866cdc9e334ed725b82bde6ba6574420bf --- tools/CMakeLists.txt (.../CMakeLists.txt) (revision cfc0df719cb5033078d0cac45ce0f6243810f2e7) +++ tools/CMakeLists.txt (.../CMakeLists.txt) (revision 70e672866cdc9e334ed725b82bde6ba6574420bf) @@ -1 +1,4 @@ -add_subdirectory(CANDumpPlayer) +add_subdirectory(AgentSim) +if(UNIX AND NOT APPLE) + add_subdirectory(CANDumpPlayer) +endif()