697 lines
26 KiB
C#
697 lines
26 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Globalization;
|
||
using System.IO;
|
||
using System.Text;
|
||
using System.Threading;
|
||
using ClumsyCore;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using FundamentalLib;
|
||
using MyParking.Shared;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
/// <summary>
|
||
/// 从C层测试界面启动定时的Detour静态定位与轮组反馈诊断记录。
|
||
/// </summary>
|
||
[MovementTest(name = "诊断：Detour静态定位记录")]
|
||
public sealed class DetourStaticDiagnosticTest : MovementTest
|
||
{
|
||
private sealed class DiagnosticSample
|
||
{
|
||
public double ElapsedSeconds;
|
||
public string LocalTimestamp;
|
||
public bool DetourReadSucceeded;
|
||
public double DetourCallDurationMilliseconds;
|
||
public long? DetourTickRaw;
|
||
public string DetourTimestamp;
|
||
public bool DetourTimestampValid;
|
||
public double? DetourDataAgeMilliseconds;
|
||
public double? DetourLStep;
|
||
public double? DetourXMillimeters;
|
||
public double? DetourYMillimeters;
|
||
public double? DetourYawDegrees;
|
||
public double? LocalDeltaMilliseconds;
|
||
public double? DetourTickDeltaMilliseconds;
|
||
public double? DeltaXMillimeters;
|
||
public double? DeltaYMillimeters;
|
||
public double? DeltaYawDegrees;
|
||
public double? DeltaPositionMillimeters;
|
||
public bool IsRepeatedTick;
|
||
public bool IsRepeatedPose;
|
||
public bool IsOutOfOrderTick;
|
||
public bool WheelReadSucceeded;
|
||
public double? WheelBodyVxMetersPerSecond;
|
||
public double? WheelBodyVyMetersPerSecond;
|
||
public double? WheelBodyOmegaDegreesPerSecond;
|
||
public double? ActualSteerLeftFrontDegrees;
|
||
public double? ActualSteerLeftRearDegrees;
|
||
public double? ActualSteerRightFrontDegrees;
|
||
public double? ActualSteerRightRearDegrees;
|
||
public double? ActualSpeedLeftFrontMetersPerSecond;
|
||
public double? ActualSpeedLeftRearMetersPerSecond;
|
||
public double? ActualSpeedRightFrontMetersPerSecond;
|
||
public double? ActualSpeedRightRearMetersPerSecond;
|
||
public string FailureReason;
|
||
}
|
||
|
||
private readonly object _sampleSyncRoot = new object();
|
||
private readonly List<DiagnosticSample> _samples =
|
||
new List<DiagnosticSample>();
|
||
private readonly Stopwatch _clock = new Stopwatch();
|
||
|
||
private MultiWheelChassis _chassis;
|
||
private Thread _samplingThread;
|
||
private volatile bool _sampling;
|
||
private int _testRunning;
|
||
private int _stopRequested;
|
||
private int _sessionId;
|
||
private bool _hasPreviousDetourSample;
|
||
private double _previousElapsedSeconds;
|
||
private long _previousDetourTick;
|
||
private double _previousDetourXMillimeters;
|
||
private double _previousDetourYMillimeters;
|
||
private double _previousDetourYawDegrees;
|
||
|
||
/// <summary>
|
||
/// 获取或设置自动结束前的记录时长，单位为min。
|
||
/// </summary>
|
||
public double DurationMinutes = 20.0;
|
||
|
||
/// <summary>
|
||
/// 获取或设置本机主动读取Detour的周期，单位为ms。
|
||
/// </summary>
|
||
public int SampleIntervalMilliseconds = 50;
|
||
|
||
/// <summary>
|
||
/// 获取最近一次静态诊断CSV的完整路径。
|
||
/// </summary>
|
||
public string SavedFilePath { get; private set; } =
|
||
string.Empty;
|
||
|
||
/// <summary>
|
||
/// 停车后开始静态采样，并在到达设定时长时自动保存CSV。
|
||
/// </summary>
|
||
public override void Test()
|
||
{
|
||
if (Interlocked.CompareExchange(
|
||
ref _testRunning,
|
||
1,
|
||
0) != 0)
|
||
{
|
||
Console.WriteLine("Detour静态诊断已经在运行。");
|
||
return;
|
||
}
|
||
|
||
var samplingStarted = false;
|
||
var completedAutomatically = false;
|
||
Exception testFailure = null;
|
||
|
||
try
|
||
{
|
||
ValidateSettings();
|
||
_chassis =
|
||
PilotDefinition.Chassis as MultiWheelChassis;
|
||
if (_chassis == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"当前底盘不是MultiWheelChassis，无法读取四轮反馈。");
|
||
}
|
||
|
||
var sessionId = ResetSession();
|
||
_chassis.PredefinedDriveStop();
|
||
_sampling = true;
|
||
_clock.Restart();
|
||
samplingStarted = true;
|
||
|
||
_samplingThread = new Thread(
|
||
() => SamplingLoop(sessionId))
|
||
{
|
||
IsBackground = true,
|
||
Name = "DetourStaticDiagnostic"
|
||
};
|
||
_samplingThread.Start();
|
||
|
||
Console.WriteLine(
|
||
$"Detour静态诊断开始：时长={DurationMinutes:F1}min，" +
|
||
$"主动读取周期={SampleIntervalMilliseconds}ms，" +
|
||
"车辆必须保持静止。");
|
||
Hedingben.ToastText(
|
||
$"Detour静态诊断开始，预计{DurationMinutes:F1}分钟后自动结束。");
|
||
|
||
var durationSeconds = DurationMinutes * 60.0;
|
||
while (Volatile.Read(ref _stopRequested) == 0 &&
|
||
_clock.Elapsed.TotalSeconds < durationSeconds)
|
||
{
|
||
Thread.Sleep(100);
|
||
}
|
||
|
||
completedAutomatically =
|
||
Volatile.Read(ref _stopRequested) == 0;
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
testFailure = exception;
|
||
Console.WriteLine(
|
||
"Detour静态诊断失败：" +
|
||
exception.Message);
|
||
}
|
||
finally
|
||
{
|
||
_sampling = false;
|
||
_chassis?.PredefinedDriveStop();
|
||
|
||
if (_samplingThread != null &&
|
||
_samplingThread != Thread.CurrentThread)
|
||
{
|
||
_samplingThread.Join(
|
||
Math.Max(
|
||
1000,
|
||
SampleIntervalMilliseconds * 4));
|
||
}
|
||
|
||
_clock.Stop();
|
||
|
||
if (samplingStarted)
|
||
{
|
||
try
|
||
{
|
||
SaveCsvAndReport(
|
||
completedAutomatically,
|
||
testFailure);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
Console.WriteLine(
|
||
"Detour静态诊断CSV保存失败：" +
|
||
exception.Message);
|
||
Hedingben.ToastText(
|
||
"Detour静态诊断CSV保存失败：" +
|
||
exception.Message);
|
||
}
|
||
}
|
||
|
||
_samplingThread = null;
|
||
_chassis = null;
|
||
Interlocked.Exchange(ref _testRunning, 0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 请求提前停止采样；测试线程随后保存已经采集的数据。
|
||
/// </summary>
|
||
public override void TestStop()
|
||
{
|
||
Interlocked.Exchange(ref _stopRequested, 1);
|
||
_sampling = false;
|
||
_chassis?.PredefinedDriveStop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除上一次测试的样本、时间基准和输出路径。
|
||
/// </summary>
|
||
private int ResetSession()
|
||
{
|
||
lock (_sampleSyncRoot)
|
||
{
|
||
_samples.Clear();
|
||
}
|
||
|
||
Interlocked.Exchange(ref _stopRequested, 0);
|
||
_hasPreviousDetourSample = false;
|
||
_previousElapsedSeconds = 0.0;
|
||
_previousDetourTick = 0;
|
||
_previousDetourXMillimeters = 0.0;
|
||
_previousDetourYMillimeters = 0.0;
|
||
_previousDetourYawDegrees = 0.0;
|
||
SavedFilePath = string.Empty;
|
||
return Interlocked.Increment(ref _sessionId);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查测试时长和主动采样周期是否适合执行。
|
||
/// </summary>
|
||
private void ValidateSettings()
|
||
{
|
||
NumericGuard.EnsureFinitePositive(
|
||
DurationMinutes,
|
||
nameof(DurationMinutes));
|
||
|
||
if (SampleIntervalMilliseconds < 20 ||
|
||
SampleIntervalMilliseconds > 5000)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(SampleIntervalMilliseconds),
|
||
"Detour主动读取周期必须在20ms到5000ms之间。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按设定周期持续采样，直至测试到时或收到停止请求。
|
||
/// </summary>
|
||
private void SamplingLoop(int sessionId)
|
||
{
|
||
while (_sampling &&
|
||
sessionId == Volatile.Read(ref _sessionId))
|
||
{
|
||
CaptureSample(sessionId);
|
||
Thread.Sleep(SampleIntervalMilliseconds);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 采集一帧原始Detour定位、接口耗时和四轮实际反馈。
|
||
/// </summary>
|
||
private void CaptureSample(int sessionId)
|
||
{
|
||
var sample = new DiagnosticSample
|
||
{
|
||
ElapsedSeconds = _clock.Elapsed.TotalSeconds,
|
||
LocalTimestamp =
|
||
DateTimeOffset.Now.ToString(
|
||
"O",
|
||
CultureInfo.InvariantCulture),
|
||
FailureReason = string.Empty
|
||
};
|
||
|
||
CaptureDetour(sample, sessionId);
|
||
|
||
if (sessionId != Volatile.Read(ref _sessionId))
|
||
{
|
||
return;
|
||
}
|
||
|
||
CaptureWheelFeedback(sample);
|
||
|
||
if (!_sampling ||
|
||
sessionId != Volatile.Read(ref _sessionId))
|
||
{
|
||
return;
|
||
}
|
||
|
||
lock (_sampleSyncRoot)
|
||
{
|
||
_samples.Add(sample);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取Detour原始字段并计算与上一成功读取之间的时间和位姿差。
|
||
/// </summary>
|
||
private void CaptureDetour(
|
||
DiagnosticSample sample,
|
||
int sessionId)
|
||
{
|
||
var callClock = Stopwatch.StartNew();
|
||
|
||
try
|
||
{
|
||
var location =
|
||
DetourInterface.getCartLocation();
|
||
callClock.Stop();
|
||
|
||
sample.DetourCallDurationMilliseconds =
|
||
callClock.Elapsed.TotalMilliseconds;
|
||
sample.DetourReadSucceeded = true;
|
||
sample.DetourTickRaw = Convert.ToInt64(
|
||
location.tick,
|
||
CultureInfo.InvariantCulture);
|
||
sample.DetourLStep = Convert.ToDouble(
|
||
location.l_step,
|
||
CultureInfo.InvariantCulture);
|
||
sample.DetourXMillimeters = Convert.ToDouble(
|
||
location.x,
|
||
CultureInfo.InvariantCulture);
|
||
sample.DetourYMillimeters = Convert.ToDouble(
|
||
location.y,
|
||
CultureInfo.InvariantCulture);
|
||
sample.DetourYawDegrees = Convert.ToDouble(
|
||
location.th,
|
||
CultureInfo.InvariantCulture);
|
||
|
||
if (sessionId != Volatile.Read(ref _sessionId))
|
||
{
|
||
return;
|
||
}
|
||
|
||
CaptureDetourTimestamp(sample);
|
||
CaptureDetourDelta(sample);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
callClock.Stop();
|
||
sample.DetourCallDurationMilliseconds =
|
||
callClock.Elapsed.TotalMilliseconds;
|
||
AppendFailure(
|
||
sample,
|
||
"Detour读取失败：" +
|
||
exception.Message);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将Detour原始tick按.NET DateTime ticks解释并记录数据年龄。
|
||
/// </summary>
|
||
private static void CaptureDetourTimestamp(
|
||
DiagnosticSample sample)
|
||
{
|
||
try
|
||
{
|
||
var detourTime = new DateTime(
|
||
sample.DetourTickRaw.Value,
|
||
DateTimeKind.Local);
|
||
sample.DetourTimestamp =
|
||
detourTime.ToString(
|
||
"O",
|
||
CultureInfo.InvariantCulture);
|
||
sample.DetourDataAgeMilliseconds =
|
||
(DateTime.Now - detourTime)
|
||
.TotalMilliseconds;
|
||
sample.DetourTimestampValid = true;
|
||
}
|
||
catch (ArgumentOutOfRangeException)
|
||
{
|
||
sample.DetourTimestamp = string.Empty;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算Detour帧间差并更新下一帧使用的原始基准。
|
||
/// </summary>
|
||
private void CaptureDetourDelta(
|
||
DiagnosticSample sample)
|
||
{
|
||
var tick = sample.DetourTickRaw.Value;
|
||
var xMillimeters = sample.DetourXMillimeters.Value;
|
||
var yMillimeters = sample.DetourYMillimeters.Value;
|
||
var yawDegrees = sample.DetourYawDegrees.Value;
|
||
|
||
if (_hasPreviousDetourSample)
|
||
{
|
||
sample.LocalDeltaMilliseconds =
|
||
(sample.ElapsedSeconds -
|
||
_previousElapsedSeconds) * 1000.0;
|
||
sample.DetourTickDeltaMilliseconds =
|
||
(tick - _previousDetourTick) /
|
||
(double)TimeSpan.TicksPerMillisecond;
|
||
sample.DeltaXMillimeters =
|
||
xMillimeters - _previousDetourXMillimeters;
|
||
sample.DeltaYMillimeters =
|
||
yMillimeters - _previousDetourYMillimeters;
|
||
sample.DeltaYawDegrees =
|
||
AngleMath.ShortestDifferenceDegrees(
|
||
yawDegrees,
|
||
_previousDetourYawDegrees);
|
||
sample.DeltaPositionMillimeters = Math.Sqrt(
|
||
sample.DeltaXMillimeters.Value *
|
||
sample.DeltaXMillimeters.Value +
|
||
sample.DeltaYMillimeters.Value *
|
||
sample.DeltaYMillimeters.Value);
|
||
sample.IsRepeatedTick =
|
||
tick == _previousDetourTick;
|
||
sample.IsOutOfOrderTick =
|
||
tick < _previousDetourTick;
|
||
sample.IsRepeatedPose =
|
||
sample.DeltaPositionMillimeters.Value <= 1e-6 &&
|
||
Math.Abs(sample.DeltaYawDegrees.Value) <= 1e-9;
|
||
}
|
||
|
||
_hasPreviousDetourSample = true;
|
||
_previousElapsedSeconds = sample.ElapsedSeconds;
|
||
_previousDetourTick = tick;
|
||
_previousDetourXMillimeters = xMillimeters;
|
||
_previousDetourYMillimeters = yMillimeters;
|
||
_previousDetourYawDegrees = yawDegrees;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取底盘反算速度与按物理安装位置识别的四轮实际反馈。
|
||
/// </summary>
|
||
private void CaptureWheelFeedback(
|
||
DiagnosticSample sample)
|
||
{
|
||
try
|
||
{
|
||
var carSpeed = _chassis.GetCarSpeed(true);
|
||
sample.WheelBodyVxMetersPerSecond = carSpeed.Vx;
|
||
sample.WheelBodyVyMetersPerSecond = carSpeed.Vy;
|
||
// CommonUsage的CarSpeed.Vw以deg/s表达。
|
||
sample.WheelBodyOmegaDegreesPerSecond = carSpeed.Vw;
|
||
|
||
#pragma warning disable CS0612, CS0618
|
||
var wheels = _chassis.GetSteerWheels();
|
||
#pragma warning restore CS0612, CS0618
|
||
|
||
var leftFront = FindWheel(wheels, true, true);
|
||
var leftRear = FindWheel(wheels, false, true);
|
||
var rightFront = FindWheel(wheels, true, false);
|
||
var rightRear = FindWheel(wheels, false, false);
|
||
|
||
if (leftFront == null || leftRear == null ||
|
||
rightFront == null || rightRear == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"未能按物理安装位置识别四个舵轮。");
|
||
}
|
||
|
||
sample.ActualSteerLeftFrontDegrees =
|
||
leftFront.ReadAngle();
|
||
sample.ActualSteerLeftRearDegrees =
|
||
leftRear.ReadAngle();
|
||
sample.ActualSteerRightFrontDegrees =
|
||
rightFront.ReadAngle();
|
||
sample.ActualSteerRightRearDegrees =
|
||
rightRear.ReadAngle();
|
||
sample.ActualSpeedLeftFrontMetersPerSecond =
|
||
leftFront.ReadSpeed();
|
||
sample.ActualSpeedLeftRearMetersPerSecond =
|
||
leftRear.ReadSpeed();
|
||
sample.ActualSpeedRightFrontMetersPerSecond =
|
||
rightFront.ReadSpeed();
|
||
sample.ActualSpeedRightRearMetersPerSecond =
|
||
rightRear.ReadSpeed();
|
||
sample.WheelReadSucceeded = true;
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
AppendFailure(
|
||
sample,
|
||
"四轮反馈读取失败：" +
|
||
exception.Message);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据真实车体X向前、Y向左的物理安装位置查找指定舵轮。
|
||
/// </summary>
|
||
private static SteerWheel FindWheel(
|
||
IReadOnlyList<SteerWheel> wheels,
|
||
bool requireFront,
|
||
bool requireLeft)
|
||
{
|
||
foreach (var wheel in wheels)
|
||
{
|
||
var isFront = wheel.PhysicalPosition.X >= 0f;
|
||
var isLeft = wheel.PhysicalPosition.Y >= 0f;
|
||
|
||
if (isFront == requireFront &&
|
||
isLeft == requireLeft)
|
||
{
|
||
return wheel;
|
||
}
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 追加本帧诊断失败原因且保留先前错误信息。
|
||
/// </summary>
|
||
private static void AppendFailure(
|
||
DiagnosticSample sample,
|
||
string reason)
|
||
{
|
||
sample.FailureReason =
|
||
string.IsNullOrWhiteSpace(sample.FailureReason)
|
||
? reason
|
||
: sample.FailureReason + "；" + reason;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存采样快照并输出自动结束或手动停止后的摘要提示。
|
||
/// </summary>
|
||
private void SaveCsvAndReport(
|
||
bool completedAutomatically,
|
||
Exception testFailure)
|
||
{
|
||
List<DiagnosticSample> snapshot;
|
||
lock (_sampleSyncRoot)
|
||
{
|
||
snapshot =
|
||
new List<DiagnosticSample>(_samples);
|
||
}
|
||
|
||
var outputDirectory = Path.Combine(
|
||
AppContext.BaseDirectory,
|
||
"DetourStaticDiagnostics");
|
||
Directory.CreateDirectory(outputDirectory);
|
||
SavedFilePath = Path.Combine(
|
||
outputDirectory,
|
||
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
|
||
"DetourStaticDiagnostic.csv");
|
||
|
||
using (var writer = new StreamWriter(
|
||
SavedFilePath,
|
||
false,
|
||
new UTF8Encoding(true)))
|
||
{
|
||
WriteCsvRow(writer,
|
||
"ElapsedSeconds", "LocalTimestamp",
|
||
"DetourReadSucceeded", "DetourCallDurationMilliseconds",
|
||
"DetourTickRaw", "DetourTimestamp",
|
||
"DetourTimestampValid", "DetourDataAgeMilliseconds",
|
||
"DetourLStep", "DetourXMillimeters",
|
||
"DetourYMillimeters", "DetourYawDegrees",
|
||
"LocalDeltaMilliseconds", "DetourTickDeltaMilliseconds",
|
||
"DeltaXMillimeters", "DeltaYMillimeters",
|
||
"DeltaYawDegrees", "DeltaPositionMillimeters",
|
||
"IsRepeatedTick", "IsRepeatedPose", "IsOutOfOrderTick",
|
||
"WheelReadSucceeded", "WheelBodyVxMetersPerSecond",
|
||
"WheelBodyVyMetersPerSecond",
|
||
"WheelBodyOmegaDegreesPerSecond",
|
||
"ActualSteerLeftFrontDegrees",
|
||
"ActualSteerLeftRearDegrees",
|
||
"ActualSteerRightFrontDegrees",
|
||
"ActualSteerRightRearDegrees",
|
||
"ActualSpeedLeftFrontMetersPerSecond",
|
||
"ActualSpeedLeftRearMetersPerSecond",
|
||
"ActualSpeedRightFrontMetersPerSecond",
|
||
"ActualSpeedRightRearMetersPerSecond",
|
||
"FailureReason");
|
||
|
||
foreach (var sample in snapshot)
|
||
{
|
||
WriteCsvRow(writer,
|
||
sample.ElapsedSeconds, sample.LocalTimestamp,
|
||
sample.DetourReadSucceeded,
|
||
sample.DetourCallDurationMilliseconds,
|
||
sample.DetourTickRaw, sample.DetourTimestamp,
|
||
sample.DetourTimestampValid,
|
||
sample.DetourDataAgeMilliseconds,
|
||
sample.DetourLStep, sample.DetourXMillimeters,
|
||
sample.DetourYMillimeters, sample.DetourYawDegrees,
|
||
sample.LocalDeltaMilliseconds,
|
||
sample.DetourTickDeltaMilliseconds,
|
||
sample.DeltaXMillimeters, sample.DeltaYMillimeters,
|
||
sample.DeltaYawDegrees,
|
||
sample.DeltaPositionMillimeters,
|
||
sample.IsRepeatedTick, sample.IsRepeatedPose,
|
||
sample.IsOutOfOrderTick,
|
||
sample.WheelReadSucceeded,
|
||
sample.WheelBodyVxMetersPerSecond,
|
||
sample.WheelBodyVyMetersPerSecond,
|
||
sample.WheelBodyOmegaDegreesPerSecond,
|
||
sample.ActualSteerLeftFrontDegrees,
|
||
sample.ActualSteerLeftRearDegrees,
|
||
sample.ActualSteerRightFrontDegrees,
|
||
sample.ActualSteerRightRearDegrees,
|
||
sample.ActualSpeedLeftFrontMetersPerSecond,
|
||
sample.ActualSpeedLeftRearMetersPerSecond,
|
||
sample.ActualSpeedRightFrontMetersPerSecond,
|
||
sample.ActualSpeedRightRearMetersPerSecond,
|
||
sample.FailureReason);
|
||
}
|
||
}
|
||
|
||
var successfulSamples = 0;
|
||
var maximumPositionStepMillimeters = 0.0;
|
||
var maximumAbsoluteYawStepDegrees = 0.0;
|
||
foreach (var sample in snapshot)
|
||
{
|
||
if (sample.DetourReadSucceeded)
|
||
{
|
||
successfulSamples++;
|
||
}
|
||
|
||
maximumPositionStepMillimeters = Math.Max(
|
||
maximumPositionStepMillimeters,
|
||
sample.DeltaPositionMillimeters ?? 0.0);
|
||
maximumAbsoluteYawStepDegrees = Math.Max(
|
||
maximumAbsoluteYawStepDegrees,
|
||
Math.Abs(sample.DeltaYawDegrees ?? 0.0));
|
||
}
|
||
|
||
var completionReason = testFailure != null
|
||
? "因异常提前结束"
|
||
: completedAutomatically
|
||
? "到达设定时长，已自动结束"
|
||
: "收到手动停止请求";
|
||
var message =
|
||
$"Detour静态诊断{completionReason}；" +
|
||
$"样本={snapshot.Count}，有效Detour样本={successfulSamples}，" +
|
||
$"最大位置阶跃={maximumPositionStepMillimeters:F2}mm，" +
|
||
$"最大航向阶跃={maximumAbsoluteYawStepDegrees:F3}°；" +
|
||
$"CSV={SavedFilePath}";
|
||
|
||
Console.WriteLine(message);
|
||
Hedingben.ToastText(message);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用InvariantCulture格式化并转义一行CSV字段。
|
||
/// </summary>
|
||
private static void WriteCsvRow(
|
||
TextWriter writer,
|
||
params object[] values)
|
||
{
|
||
var fields = new string[values.Length];
|
||
for (var index = 0; index < values.Length; index++)
|
||
{
|
||
fields[index] = FormatCsvValue(values[index]);
|
||
}
|
||
|
||
writer.WriteLine(string.Join(",", fields));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将单个值转换为区域无关且符合CSV转义规则的文本。
|
||
/// </summary>
|
||
private static string FormatCsvValue(object value)
|
||
{
|
||
if (value == null)
|
||
{
|
||
return string.Empty;
|
||
}
|
||
|
||
string text;
|
||
if (value is bool boolean)
|
||
{
|
||
text = boolean ? "1" : "0";
|
||
}
|
||
else if (value is IFormattable formattable)
|
||
{
|
||
text = formattable.ToString(
|
||
null,
|
||
CultureInfo.InvariantCulture);
|
||
}
|
||
else
|
||
{
|
||
text = value.ToString();
|
||
}
|
||
|
||
if (text.IndexOfAny(
|
||
new[] { ',', '"', '\r', '\n' }) < 0)
|
||
{
|
||
return text;
|
||
}
|
||
|
||
return "\"" +
|
||
text.Replace("\"", "\"\"") +
|
||
"\"";
|
||
}
|
||
}
|
||
}
|