655 lines
25 KiB
C#
655 lines
25 KiB
C#
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 _snapshotSequence;
|
|
private long _droppedRecordCount;
|
|
private double _lastSnapshotMilliseconds = double.NegativeInfinity;
|
|
private long _startTimestamp;
|
|
|
|
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,EventType,ChannelName," +
|
|
"RawRpm,SpeedMps,PositionMm,RawAngle,AngleDegrees");
|
|
|
|
_snapshotWriter.WriteLine(
|
|
"ElapsedMs,SnapshotSequence," +
|
|
"ControlElapsedMs,ControlSequence,ControlAgeMs," +
|
|
"WheelCommandElapsedMs,WheelCommandSequence,WheelCommandAgeMs," +
|
|
"CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
|
|
"VoltageV,AlarmLevel,ChassisMode,WheelAbleState," +
|
|
"DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
|
|
"DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," +
|
|
"FeedforwardDeltaTimeMs," +
|
|
"TargetRateThLeftFrontDegreesPerSecond,TargetRateThLeftRearDegreesPerSecond," +
|
|
"TargetRateThRightFrontDegreesPerSecond,TargetRateThRightRearDegreesPerSecond," +
|
|
"FeedforwardLimitedLeftFront,FeedforwardLimitedLeftRear,FeedforwardLimitedRightFront,FeedforwardLimitedRightRear," +
|
|
"PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
|
|
"RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," +
|
|
"TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," +
|
|
"CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
|
|
"PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
|
|
"SentLFLMps,SentLFRMps,SentLRLMps,SentLRRMps,SentRFLMps,SentRFRMps,SentRRLMps,SentRRRMps," +
|
|
"CommandLimitedLFL,CommandLimitedLFR,CommandLimitedLRL,CommandLimitedLRR," +
|
|
"CommandLimitedRFL,CommandLimitedRFR,CommandLimitedRRL,CommandLimitedRRR," +
|
|
"PairCommandLimitedLeftFront,PairCommandLimitedLeftRear," +
|
|
"PairCommandLimitedRightFront,PairCommandLimitedRightRear,WheelCommandSuppressed," +
|
|
"ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
|
|
"ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," +
|
|
"PositionLFL,PositionLFR,PositionLRL,PositionLRR,PositionRFL,PositionRFR,PositionRRL,PositionRRR," +
|
|
"CurrentLFLAmps,CurrentLFRAmps,CurrentLRLAmps,CurrentLRRAmps," +
|
|
"CurrentRFLAmps,CurrentRFRAmps,CurrentRRLAmps,CurrentRRRAmps," +
|
|
"TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," +
|
|
"ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," +
|
|
"ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear," +
|
|
"ActualThLeftFrontReceiveElapsedMs,ActualThLeftFrontReceiveSequence,ActualThLeftFrontAgeMs," +
|
|
"ActualThLeftRearReceiveElapsedMs,ActualThLeftRearReceiveSequence,ActualThLeftRearAgeMs," +
|
|
"ActualThRightFrontReceiveElapsedMs,ActualThRightFrontReceiveSequence,ActualThRightFrontAgeMs," +
|
|
"ActualThRightRearReceiveElapsedMs,ActualThRightRearReceiveSequence,ActualThRightRearAgeMs");
|
|
|
|
while (_records.TryDequeue(out _))
|
|
{
|
|
}
|
|
|
|
_queuedRecordCount = 0;
|
|
_receiveSequence = 0;
|
|
_snapshotSequence = 0;
|
|
_droppedRecordCount = 0;
|
|
_lastSnapshotMilliseconds =
|
|
double.NegativeInfinity;
|
|
_startTimestamp = Stopwatch.GetTimestamp();
|
|
_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,
|
|
float positionMillimeters)
|
|
{
|
|
if (!_isRunning)
|
|
return;
|
|
|
|
var elapsedMilliseconds =
|
|
GetElapsedMilliseconds(
|
|
Stopwatch.GetTimestamp());
|
|
var receiveSequence =
|
|
Interlocked.Increment(
|
|
ref _receiveSequence);
|
|
|
|
var line = string.Join(
|
|
",",
|
|
Format(elapsedMilliseconds),
|
|
receiveSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
$"0x{canId:X3}",
|
|
"MotorSpeedPosition",
|
|
motorName,
|
|
Format(rawRpm),
|
|
Format(speedMetersPerSecond),
|
|
Format(positionMillimeters),
|
|
"",
|
|
"");
|
|
|
|
Enqueue(new LogRecord(
|
|
isCanEvent: true,
|
|
line));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 按CAN回调到达时刻记录一帧舵角原始值和换算后的机械角度。
|
|
/// </summary>
|
|
public void RecordSteeringAngleFeedback(
|
|
ushort canId,
|
|
string wheelName,
|
|
int rawAngle,
|
|
float angleDegrees)
|
|
{
|
|
if (!_isRunning)
|
|
return;
|
|
|
|
var elapsedMilliseconds =
|
|
GetElapsedMilliseconds(
|
|
Stopwatch.GetTimestamp());
|
|
var receiveSequence =
|
|
Interlocked.Increment(
|
|
ref _receiveSequence);
|
|
|
|
var line = string.Join(
|
|
",",
|
|
Format(elapsedMilliseconds),
|
|
receiveSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
$"0x{canId:X3}",
|
|
"SteeringAngle",
|
|
wheelName,
|
|
"",
|
|
"",
|
|
"",
|
|
rawAngle.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
Format(angleDegrees));
|
|
|
|
Enqueue(new LogRecord(
|
|
isCanEvent: true,
|
|
line));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。
|
|
/// </summary>
|
|
public void RecordSnapshot(
|
|
DiverCartDefinition cart)
|
|
{
|
|
if (!_isRunning || cart == null)
|
|
return;
|
|
|
|
var snapshotTimestamp = Stopwatch.GetTimestamp();
|
|
var elapsedMilliseconds =
|
|
GetElapsedMilliseconds(snapshotTimestamp);
|
|
|
|
if (elapsedMilliseconds -
|
|
_lastSnapshotMilliseconds <
|
|
SnapshotIntervalMilliseconds)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_lastSnapshotMilliseconds =
|
|
elapsedMilliseconds;
|
|
|
|
var snapshotSequence =
|
|
Interlocked.Increment(
|
|
ref _snapshotSequence);
|
|
var controlTimestamp =
|
|
Interlocked.Read(
|
|
ref cart.DiffSteerControlTimestamp);
|
|
var controlSequence =
|
|
Interlocked.Read(
|
|
ref cart.DiffSteerControlSequence);
|
|
var commandTimestamp =
|
|
Interlocked.Read(
|
|
ref cart.WheelCommandTimestamp);
|
|
var commandSequence =
|
|
Interlocked.Read(
|
|
ref cart.WheelCommandSequence);
|
|
var actualThLeftFrontTimestamp =
|
|
Interlocked.Read(
|
|
ref cart.ActualThLeftFrontTimestamp);
|
|
var actualThLeftFrontSequence =
|
|
Interlocked.Read(
|
|
ref cart.ActualThLeftFrontSequence);
|
|
var actualThLeftRearTimestamp =
|
|
Interlocked.Read(
|
|
ref cart.ActualThLeftRearTimestamp);
|
|
var actualThLeftRearSequence =
|
|
Interlocked.Read(
|
|
ref cart.ActualThLeftRearSequence);
|
|
var actualThRightFrontTimestamp =
|
|
Interlocked.Read(
|
|
ref cart.ActualThRightFrontTimestamp);
|
|
var actualThRightFrontSequence =
|
|
Interlocked.Read(
|
|
ref cart.ActualThRightFrontSequence);
|
|
var actualThRightRearTimestamp =
|
|
Interlocked.Read(
|
|
ref cart.ActualThRightRearTimestamp);
|
|
var actualThRightRearSequence =
|
|
Interlocked.Read(
|
|
ref cart.ActualThRightRearSequence);
|
|
|
|
var line = string.Join(
|
|
",",
|
|
Format(elapsedMilliseconds),
|
|
snapshotSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
FormatEventElapsedMilliseconds(controlTimestamp),
|
|
controlSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
FormatEventAgeMilliseconds(
|
|
snapshotTimestamp,
|
|
controlTimestamp),
|
|
FormatEventElapsedMilliseconds(commandTimestamp),
|
|
commandSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
FormatEventAgeMilliseconds(
|
|
snapshotTimestamp,
|
|
commandTimestamp),
|
|
cart.CarNum.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
Format((int)cart.TransmitterControlMode),
|
|
Format(cart.ManualMode),
|
|
Format(cart.SendThresSpeed),
|
|
Format(cart.Voltage),
|
|
Format(cart.AlarmLevel),
|
|
Format(cart.ChassisMode),
|
|
FormatBoolean(cart.WheelAbleState),
|
|
Format(cart.DiffSteerKp),
|
|
Format(cart.DiffSteerKi),
|
|
Format(cart.DiffSteerKd),
|
|
Format(cart.DiffSteerMaxI),
|
|
Format(cart.DiffSteerDeadZone),
|
|
Format(cart.DiffSteerThresh),
|
|
Format(cart.DiffSteerSpeedAcc),
|
|
Format(cart.DiffSteerRateFeedforwardGain),
|
|
Format(cart.DiffSteerWheelDistanceMillimeters),
|
|
Format(cart.DiffSteerRateFeedforwardMaximumSpeed),
|
|
Format(cart.DiffSteerFeedforwardDeltaTimeMilliseconds),
|
|
Format(cart.DiffSteerTargetRateLeftFrontDegreesPerSecond),
|
|
Format(cart.DiffSteerTargetRateLeftRearDegreesPerSecond),
|
|
Format(cart.DiffSteerTargetRateRightFrontDegreesPerSecond),
|
|
Format(cart.DiffSteerTargetRateRightRearDegreesPerSecond),
|
|
FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftFront),
|
|
FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftRear),
|
|
FormatBoolean(cart.DiffSteerFeedforwardLimitedRightFront),
|
|
FormatBoolean(cart.DiffSteerFeedforwardLimitedRightRear),
|
|
Format(cart.DiffSteerOutputLeftFront),
|
|
Format(cart.DiffSteerOutputLeftRear),
|
|
Format(cart.DiffSteerOutputRightFront),
|
|
Format(cart.DiffSteerOutputRightRear),
|
|
Format(cart.DiffSteerRateFeedforwardLeftFront),
|
|
Format(cart.DiffSteerRateFeedforwardLeftRear),
|
|
Format(cart.DiffSteerRateFeedforwardRightFront),
|
|
Format(cart.DiffSteerRateFeedforwardRightRear),
|
|
Format(cart.DiffSteerTotalOutputLeftFront),
|
|
Format(cart.DiffSteerTotalOutputLeftRear),
|
|
Format(cart.DiffSteerTotalOutputRightFront),
|
|
Format(cart.DiffSteerTotalOutputRightRear),
|
|
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.SentSpeedLFL),
|
|
Format(cart.SentSpeedLFR),
|
|
Format(cart.SentSpeedLRL),
|
|
Format(cart.SentSpeedLRR),
|
|
Format(cart.SentSpeedRFL),
|
|
Format(cart.SentSpeedRFR),
|
|
Format(cart.SentSpeedRRL),
|
|
Format(cart.SentSpeedRRR),
|
|
FormatBoolean(cart.WheelCommandLimitedLFL),
|
|
FormatBoolean(cart.WheelCommandLimitedLFR),
|
|
FormatBoolean(cart.WheelCommandLimitedLRL),
|
|
FormatBoolean(cart.WheelCommandLimitedLRR),
|
|
FormatBoolean(cart.WheelCommandLimitedRFL),
|
|
FormatBoolean(cart.WheelCommandLimitedRFR),
|
|
FormatBoolean(cart.WheelCommandLimitedRRL),
|
|
FormatBoolean(cart.WheelCommandLimitedRRR),
|
|
FormatBoolean(
|
|
cart.WheelCommandLimitedLFL ||
|
|
cart.WheelCommandLimitedLFR),
|
|
FormatBoolean(
|
|
cart.WheelCommandLimitedLRL ||
|
|
cart.WheelCommandLimitedLRR),
|
|
FormatBoolean(
|
|
cart.WheelCommandLimitedRFL ||
|
|
cart.WheelCommandLimitedRFR),
|
|
FormatBoolean(
|
|
cart.WheelCommandLimitedRRL ||
|
|
cart.WheelCommandLimitedRRR),
|
|
FormatBoolean(cart.WheelCommandSuppressed),
|
|
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.LFLActualPos),
|
|
Format(cart.LFRActualPos),
|
|
Format(cart.LRLActualPos),
|
|
Format(cart.LRRActualPos),
|
|
Format(cart.RFLActualPos),
|
|
Format(cart.RFRActualPos),
|
|
Format(cart.RRLActualPos),
|
|
Format(cart.RRRActualPos),
|
|
Format(cart.LeftFrontLeftElectric),
|
|
Format(cart.LeftFrontRightElectric),
|
|
Format(cart.LeftRearLeftElectric),
|
|
Format(cart.LeftRearRightElectric),
|
|
Format(cart.RightFrontLeftElectric),
|
|
Format(cart.RightFrontRightElectric),
|
|
Format(cart.RightRearLeftElectric),
|
|
Format(cart.RightRearRightElectric),
|
|
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),
|
|
FormatEventElapsedMilliseconds(
|
|
actualThLeftFrontTimestamp),
|
|
actualThLeftFrontSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
FormatEventAgeMilliseconds(
|
|
snapshotTimestamp,
|
|
actualThLeftFrontTimestamp),
|
|
FormatEventElapsedMilliseconds(
|
|
actualThLeftRearTimestamp),
|
|
actualThLeftRearSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
FormatEventAgeMilliseconds(
|
|
snapshotTimestamp,
|
|
actualThLeftRearTimestamp),
|
|
FormatEventElapsedMilliseconds(
|
|
actualThRightFrontTimestamp),
|
|
actualThRightFrontSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
FormatEventAgeMilliseconds(
|
|
snapshotTimestamp,
|
|
actualThRightFrontTimestamp),
|
|
FormatEventElapsedMilliseconds(
|
|
actualThRightRearTimestamp),
|
|
actualThRightRearSequence.ToString(
|
|
CultureInfo.InvariantCulture),
|
|
FormatEventAgeMilliseconds(
|
|
snapshotTimestamp,
|
|
actualThRightRearTimestamp));
|
|
|
|
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);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将诊断布尔值写成便于MATLAB直接读取的0或1。
|
|
/// </summary>
|
|
private static string FormatBoolean(bool value)
|
|
{
|
|
return value ? "1" : "0";
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将本机单调时钟值换算为相对本次日志开始的毫秒数。
|
|
/// </summary>
|
|
private double GetElapsedMilliseconds(long timestamp)
|
|
{
|
|
return (timestamp - _startTimestamp) *
|
|
1000.0 /
|
|
Stopwatch.Frequency;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 格式化发生在本次记录期间的事件时刻，记录前事件返回空字段。
|
|
/// </summary>
|
|
private string FormatEventElapsedMilliseconds(long timestamp)
|
|
{
|
|
if (timestamp < _startTimestamp)
|
|
return "";
|
|
|
|
return Format(GetElapsedMilliseconds(timestamp));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 计算快照时刻相对最近一次控制或反馈事件的数据年龄。
|
|
/// </summary>
|
|
private static string FormatEventAgeMilliseconds(
|
|
long currentTimestamp,
|
|
long eventTimestamp)
|
|
{
|
|
if (eventTimestamp <= 0 ||
|
|
eventTimestamp > currentTimestamp)
|
|
{
|
|
return "";
|
|
}
|
|
|
|
return Format(
|
|
(currentTimestamp - eventTimestamp) *
|
|
1000.0 /
|
|
Stopwatch.Frequency);
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
Stop();
|
|
_recordsAvailable.Dispose();
|
|
}
|
|
}
|
|
}
|