完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -38,8 +38,10 @@ namespace MedullaAdapter
|
||||
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;
|
||||
|
||||
@@ -79,22 +81,43 @@ namespace MedullaAdapter
|
||||
_snapshotWriter = CreateWriter(SnapshotLogPath);
|
||||
|
||||
_canWriter.WriteLine(
|
||||
"ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps");
|
||||
"ElapsedMs,ReceiveSequence,CanId,EventType,ChannelName," +
|
||||
"RawRpm,SpeedMps,PositionMm,RawAngle,AngleDegrees");
|
||||
|
||||
_snapshotWriter.WriteLine(
|
||||
"ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
|
||||
"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");
|
||||
"ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear," +
|
||||
"ActualThLeftFrontReceiveElapsedMs,ActualThLeftFrontReceiveSequence,ActualThLeftFrontAgeMs," +
|
||||
"ActualThLeftRearReceiveElapsedMs,ActualThLeftRearReceiveSequence,ActualThLeftRearAgeMs," +
|
||||
"ActualThRightFrontReceiveElapsedMs,ActualThRightFrontReceiveSequence,ActualThRightFrontAgeMs," +
|
||||
"ActualThRightRearReceiveElapsedMs,ActualThRightRearReceiveSequence,ActualThRightRearAgeMs");
|
||||
|
||||
while (_records.TryDequeue(out _))
|
||||
{
|
||||
@@ -102,9 +125,11 @@ namespace MedullaAdapter
|
||||
|
||||
_queuedRecordCount = 0;
|
||||
_receiveSequence = 0;
|
||||
_snapshotSequence = 0;
|
||||
_droppedRecordCount = 0;
|
||||
_lastSnapshotMilliseconds =
|
||||
double.NegativeInfinity;
|
||||
_startTimestamp = Stopwatch.GetTimestamp();
|
||||
_stopwatch = Stopwatch.StartNew();
|
||||
_isRunning = true;
|
||||
|
||||
@@ -157,13 +182,15 @@ namespace MedullaAdapter
|
||||
ushort canId,
|
||||
string motorName,
|
||||
float rawRpm,
|
||||
float speedMetersPerSecond)
|
||||
float speedMetersPerSecond,
|
||||
float positionMillimeters)
|
||||
{
|
||||
if (!_isRunning)
|
||||
return;
|
||||
|
||||
var elapsedMilliseconds =
|
||||
_stopwatch.Elapsed.TotalMilliseconds;
|
||||
GetElapsedMilliseconds(
|
||||
Stopwatch.GetTimestamp());
|
||||
var receiveSequence =
|
||||
Interlocked.Increment(
|
||||
ref _receiveSequence);
|
||||
@@ -174,9 +201,52 @@ namespace MedullaAdapter
|
||||
receiveSequence.ToString(
|
||||
CultureInfo.InvariantCulture),
|
||||
$"0x{canId:X3}",
|
||||
"MotorSpeedPosition",
|
||||
motorName,
|
||||
Format(rawRpm),
|
||||
Format(speedMetersPerSecond));
|
||||
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,
|
||||
@@ -192,8 +262,9 @@ namespace MedullaAdapter
|
||||
if (!_isRunning || cart == null)
|
||||
return;
|
||||
|
||||
var snapshotTimestamp = Stopwatch.GetTimestamp();
|
||||
var elapsedMilliseconds =
|
||||
_stopwatch.Elapsed.TotalMilliseconds;
|
||||
GetElapsedMilliseconds(snapshotTimestamp);
|
||||
|
||||
if (elapsedMilliseconds -
|
||||
_lastSnapshotMilliseconds <
|
||||
@@ -205,14 +276,72 @@ namespace MedullaAdapter
|
||||
_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),
|
||||
@@ -223,6 +352,15 @@ namespace MedullaAdapter
|
||||
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),
|
||||
@@ -251,6 +389,35 @@ namespace MedullaAdapter
|
||||
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),
|
||||
@@ -263,6 +430,22 @@ namespace MedullaAdapter
|
||||
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),
|
||||
@@ -274,7 +457,35 @@ namespace MedullaAdapter
|
||||
Format(cart.ThLeftFront - cart.ActualThLeftFront),
|
||||
Format(cart.ThLeftRear - cart.ActualThLeftRear),
|
||||
Format(cart.ThRightFront - cart.ActualThRightFront),
|
||||
Format(cart.ThRightRear - cart.ActualThRightRear));
|
||||
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,
|
||||
@@ -386,6 +597,54 @@ namespace MedullaAdapter
|
||||
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();
|
||||
|
||||
Reference in New Issue
Block a user