Initial commit from MyParking project
This commit is contained in:
@@ -0,0 +1,381 @@
|
||||
using System;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
/// <summary>
|
||||
/// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。
|
||||
/// </summary>
|
||||
internal sealed class WheelSpeedDiagnosticLogger : IDisposable
|
||||
{
|
||||
private readonly struct LogRecord
|
||||
{
|
||||
public LogRecord(bool isCanEvent, string line)
|
||||
{
|
||||
IsCanEvent = isCanEvent;
|
||||
Line = line;
|
||||
}
|
||||
|
||||
public bool IsCanEvent { get; }
|
||||
|
||||
public string Line { get; }
|
||||
}
|
||||
|
||||
private const int MaximumQueuedRecords = 100000;
|
||||
private const double SnapshotIntervalMilliseconds = 20.0;
|
||||
private readonly ConcurrentQueue<LogRecord> _records = new();
|
||||
private readonly AutoResetEvent _recordsAvailable = new(false);
|
||||
private readonly object _lifecycleLock = new();
|
||||
private Stopwatch _stopwatch;
|
||||
private Thread _writerThread;
|
||||
private StreamWriter _canWriter;
|
||||
private StreamWriter _snapshotWriter;
|
||||
private volatile bool _isRunning;
|
||||
private int _queuedRecordCount;
|
||||
private long _receiveSequence;
|
||||
private long _droppedRecordCount;
|
||||
private double _lastSnapshotMilliseconds = double.NegativeInfinity;
|
||||
|
||||
public bool IsRunning => _isRunning;
|
||||
|
||||
public string CanLogPath { get; private set; } = "";
|
||||
|
||||
public string SnapshotLogPath { get; private set; } = "";
|
||||
|
||||
/// <summary>
|
||||
/// 创建本次诊断的两个CSV文件并启动后台写入线程。
|
||||
/// </summary>
|
||||
public void Start(string directory, int carNumber)
|
||||
{
|
||||
lock (_lifecycleLock)
|
||||
{
|
||||
if (_isRunning)
|
||||
return;
|
||||
|
||||
if (string.IsNullOrWhiteSpace(directory))
|
||||
throw new ArgumentException(
|
||||
"轮速诊断目录不能为空。",
|
||||
nameof(directory));
|
||||
|
||||
Directory.CreateDirectory(directory);
|
||||
|
||||
var filePrefix =
|
||||
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_Car{carNumber}";
|
||||
|
||||
CanLogPath = Path.Combine(
|
||||
directory,
|
||||
$"{filePrefix}_can.csv");
|
||||
|
||||
SnapshotLogPath = Path.Combine(
|
||||
directory,
|
||||
$"{filePrefix}_snapshot.csv");
|
||||
|
||||
_canWriter = CreateWriter(CanLogPath);
|
||||
_snapshotWriter = CreateWriter(SnapshotLogPath);
|
||||
|
||||
_canWriter.WriteLine(
|
||||
"ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps");
|
||||
|
||||
_snapshotWriter.WriteLine(
|
||||
"ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
|
||||
"DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
|
||||
"PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
|
||||
"CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
|
||||
"PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
|
||||
"ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
|
||||
"ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," +
|
||||
"TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," +
|
||||
"ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," +
|
||||
"ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear");
|
||||
|
||||
while (_records.TryDequeue(out _))
|
||||
{
|
||||
}
|
||||
|
||||
_queuedRecordCount = 0;
|
||||
_receiveSequence = 0;
|
||||
_droppedRecordCount = 0;
|
||||
_lastSnapshotMilliseconds =
|
||||
double.NegativeInfinity;
|
||||
_stopwatch = Stopwatch.StartNew();
|
||||
_isRunning = true;
|
||||
|
||||
_writerThread = new Thread(WriterLoop)
|
||||
{
|
||||
IsBackground = true,
|
||||
Name = "WheelSpeedDiagnosticWriter"
|
||||
};
|
||||
_writerThread.Start();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止记录并等待队列中的诊断数据写入磁盘。
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
Thread writerThread;
|
||||
|
||||
lock (_lifecycleLock)
|
||||
{
|
||||
if (!_isRunning &&
|
||||
_writerThread == null)
|
||||
return;
|
||||
|
||||
_isRunning = false;
|
||||
writerThread = _writerThread;
|
||||
_recordsAvailable.Set();
|
||||
}
|
||||
|
||||
writerThread?.Join(3000);
|
||||
|
||||
lock (_lifecycleLock)
|
||||
{
|
||||
_canWriter?.Flush();
|
||||
_snapshotWriter?.Flush();
|
||||
_canWriter?.Dispose();
|
||||
_snapshotWriter?.Dispose();
|
||||
_canWriter = null;
|
||||
_snapshotWriter = null;
|
||||
_writerThread = null;
|
||||
_stopwatch?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。
|
||||
/// </summary>
|
||||
public void RecordCanFeedback(
|
||||
ushort canId,
|
||||
string motorName,
|
||||
float rawRpm,
|
||||
float speedMetersPerSecond)
|
||||
{
|
||||
if (!_isRunning)
|
||||
return;
|
||||
|
||||
var elapsedMilliseconds =
|
||||
_stopwatch.Elapsed.TotalMilliseconds;
|
||||
var receiveSequence =
|
||||
Interlocked.Increment(
|
||||
ref _receiveSequence);
|
||||
|
||||
var line = string.Join(
|
||||
",",
|
||||
Format(elapsedMilliseconds),
|
||||
receiveSequence.ToString(
|
||||
CultureInfo.InvariantCulture),
|
||||
$"0x{canId:X3}",
|
||||
motorName,
|
||||
Format(rawRpm),
|
||||
Format(speedMetersPerSecond));
|
||||
|
||||
Enqueue(new LogRecord(
|
||||
isCanEvent: true,
|
||||
line));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。
|
||||
/// </summary>
|
||||
public void RecordSnapshot(
|
||||
DiverCartDefinition cart)
|
||||
{
|
||||
if (!_isRunning || cart == null)
|
||||
return;
|
||||
|
||||
var elapsedMilliseconds =
|
||||
_stopwatch.Elapsed.TotalMilliseconds;
|
||||
|
||||
if (elapsedMilliseconds -
|
||||
_lastSnapshotMilliseconds <
|
||||
SnapshotIntervalMilliseconds)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_lastSnapshotMilliseconds =
|
||||
elapsedMilliseconds;
|
||||
|
||||
var line = string.Join(
|
||||
",",
|
||||
Format(elapsedMilliseconds),
|
||||
cart.CarNum.ToString(
|
||||
CultureInfo.InvariantCulture),
|
||||
Format((int)cart.TransmitterControlMode),
|
||||
Format(cart.ManualMode),
|
||||
Format(cart.SendThresSpeed),
|
||||
Format(cart.DiffSteerKp),
|
||||
Format(cart.DiffSteerKi),
|
||||
Format(cart.DiffSteerKd),
|
||||
Format(cart.DiffSteerMaxI),
|
||||
Format(cart.DiffSteerDeadZone),
|
||||
Format(cart.DiffSteerThresh),
|
||||
Format(cart.DiffSteerSpeedAcc),
|
||||
Format(cart.DiffSteerOutputLeftFront),
|
||||
Format(cart.DiffSteerOutputLeftRear),
|
||||
Format(cart.DiffSteerOutputRightFront),
|
||||
Format(cart.DiffSteerOutputRightRear),
|
||||
Format(cart.SpeedLeftFrontLeft),
|
||||
Format(cart.SpeedLeftFrontRight),
|
||||
Format(cart.SpeedLeftRearLeft),
|
||||
Format(cart.SpeedLeftRearRight),
|
||||
Format(cart.SpeedRightFrontLeft),
|
||||
Format(cart.SpeedRightFrontRight),
|
||||
Format(cart.SpeedRightRearLeft),
|
||||
Format(cart.SpeedRightRearRight),
|
||||
Format(cart.SpeedLFL),
|
||||
Format(cart.SpeedLFR),
|
||||
Format(cart.SpeedLRL),
|
||||
Format(cart.SpeedLRR),
|
||||
Format(cart.SpeedRFL),
|
||||
Format(cart.SpeedRFR),
|
||||
Format(cart.SpeedRRL),
|
||||
Format(cart.SpeedRRR),
|
||||
Format(cart.ActualSpeedLeftFrontLeft),
|
||||
Format(cart.ActualSpeedLeftFrontRight),
|
||||
Format(cart.ActualSpeedLeftRearLeft),
|
||||
Format(cart.ActualSpeedLeftRearRight),
|
||||
Format(cart.ActualSpeedRightFrontLeft),
|
||||
Format(cart.ActualSpeedRightFrontRight),
|
||||
Format(cart.ActualSpeedRightRearLeft),
|
||||
Format(cart.ActualSpeedRightRearRight),
|
||||
Format(cart.ActualSpeedLeftFront),
|
||||
Format(cart.ActualSpeedLeftRear),
|
||||
Format(cart.ActualSpeedRightFront),
|
||||
Format(cart.ActualSpeedRightRear),
|
||||
Format(cart.ThLeftFront),
|
||||
Format(cart.ThLeftRear),
|
||||
Format(cart.ThRightFront),
|
||||
Format(cart.ThRightRear),
|
||||
Format(cart.ActualThLeftFront),
|
||||
Format(cart.ActualThLeftRear),
|
||||
Format(cart.ActualThRightFront),
|
||||
Format(cart.ActualThRightRear),
|
||||
Format(cart.ThLeftFront - cart.ActualThLeftFront),
|
||||
Format(cart.ThLeftRear - cart.ActualThLeftRear),
|
||||
Format(cart.ThRightFront - cart.ActualThRightFront),
|
||||
Format(cart.ThRightRear - cart.ActualThRightRear));
|
||||
|
||||
Enqueue(new LogRecord(
|
||||
isCanEvent: false,
|
||||
line));
|
||||
}
|
||||
|
||||
private static StreamWriter CreateWriter(
|
||||
string path)
|
||||
{
|
||||
return new StreamWriter(
|
||||
path,
|
||||
append: false,
|
||||
new UTF8Encoding(
|
||||
encoderShouldEmitUTF8Identifier: true),
|
||||
bufferSize: 64 * 1024);
|
||||
}
|
||||
|
||||
private void Enqueue(LogRecord record)
|
||||
{
|
||||
var queuedCount =
|
||||
Interlocked.Increment(
|
||||
ref _queuedRecordCount);
|
||||
|
||||
if (queuedCount >
|
||||
MaximumQueuedRecords)
|
||||
{
|
||||
Interlocked.Decrement(
|
||||
ref _queuedRecordCount);
|
||||
Interlocked.Increment(
|
||||
ref _droppedRecordCount);
|
||||
return;
|
||||
}
|
||||
|
||||
_records.Enqueue(record);
|
||||
_recordsAvailable.Set();
|
||||
}
|
||||
|
||||
private void WriterLoop()
|
||||
{
|
||||
var lastFlushTime = DateTime.UtcNow;
|
||||
|
||||
try
|
||||
{
|
||||
while (_isRunning ||
|
||||
!_records.IsEmpty)
|
||||
{
|
||||
var wroteAnyRecord = false;
|
||||
|
||||
while (_records.TryDequeue(
|
||||
out var record))
|
||||
{
|
||||
Interlocked.Decrement(
|
||||
ref _queuedRecordCount);
|
||||
|
||||
if (record.IsCanEvent)
|
||||
_canWriter.WriteLine(record.Line);
|
||||
else
|
||||
_snapshotWriter.WriteLine(record.Line);
|
||||
|
||||
wroteAnyRecord = true;
|
||||
}
|
||||
|
||||
var shouldFlush =
|
||||
wroteAnyRecord &&
|
||||
(DateTime.UtcNow -
|
||||
lastFlushTime)
|
||||
.TotalMilliseconds >= 500.0;
|
||||
|
||||
if (shouldFlush)
|
||||
{
|
||||
_canWriter.Flush();
|
||||
_snapshotWriter.Flush();
|
||||
lastFlushTime = DateTime.UtcNow;
|
||||
}
|
||||
|
||||
if (!wroteAnyRecord)
|
||||
_recordsAvailable.WaitOne(100);
|
||||
}
|
||||
|
||||
var dropped =
|
||||
Interlocked.Read(
|
||||
ref _droppedRecordCount);
|
||||
|
||||
if (dropped > 0)
|
||||
{
|
||||
_canWriter.WriteLine(
|
||||
$"# DroppedRecords={dropped}");
|
||||
_snapshotWriter.WriteLine(
|
||||
$"# DroppedRecords={dropped}");
|
||||
}
|
||||
|
||||
_canWriter.Flush();
|
||||
_snapshotWriter.Flush();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// 后台日志失败不能终止车辆控制线程。
|
||||
Console.WriteLine(
|
||||
"轮速诊断后台写入失败：" +
|
||||
ex.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private static string Format(
|
||||
double value)
|
||||
{
|
||||
return value.ToString(
|
||||
"0.######",
|
||||
CultureInfo.InvariantCulture);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Stop();
|
||||
_recordsAvailable.Dispose();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user