First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善状态估计、车队协调与舵轮辨识日志

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-24 18:39:41 +08:00
parent 0ab409cd2a
commit 95c0b19a26
40 changed files with 5983 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/DiverCartDefinition.cs
+43 -1
View File
@@ -66,7 +66,8 @@ namespace MedullaAdapter
#region 初始参数
[AsInitParam(desc = "差速转舵目标角速度前馈增益")]
public float DiffSteerRateFeedforwardGain = 0.9f;
// public float DiffSteerRateFeedforwardGain = 0.9f;
public float DiffSteerRateFeedforwardGain = 0f;
[AsInitParam(desc = "差速舵轮左右轮间距，单位mm")]
public float DiffSteerWheelDistanceMillimeters = 85f;
@@ -124,6 +125,47 @@ namespace MedullaAdapter
public bool WheelSpeedDiagnosticEnabled;
[IOObjectMonitor(desc = "轮速诊断记录状态")]
public string WheelSpeedDiagnosticStatus = "未启动";
// M层辨识日志：以下字段只保存控制和反馈事件的单调时钟快照，
// 不参与底盘控制、限幅或模式切换。
internal long DiffSteerControlTimestamp;
internal long DiffSteerControlSequence;
internal long WheelCommandTimestamp;
internal long WheelCommandSequence;
internal float SentSpeedLFL;
internal float SentSpeedLFR;
internal float SentSpeedLRL;
internal float SentSpeedLRR;
internal float SentSpeedRFL;
internal float SentSpeedRFR;
internal float SentSpeedRRL;
internal float SentSpeedRRR;
internal bool WheelCommandLimitedLFL;
internal bool WheelCommandLimitedLFR;
internal bool WheelCommandLimitedLRL;
internal bool WheelCommandLimitedLRR;
internal bool WheelCommandLimitedRFL;
internal bool WheelCommandLimitedRFR;
internal bool WheelCommandLimitedRRL;
internal bool WheelCommandLimitedRRR;
internal bool WheelCommandSuppressed;
internal float DiffSteerTargetRateLeftFrontDegreesPerSecond;
internal float DiffSteerTargetRateLeftRearDegreesPerSecond;
internal float DiffSteerTargetRateRightFrontDegreesPerSecond;
internal float DiffSteerTargetRateRightRearDegreesPerSecond;
internal float DiffSteerFeedforwardDeltaTimeMilliseconds;
internal bool DiffSteerFeedforwardLimitedLeftFront;
internal bool DiffSteerFeedforwardLimitedLeftRear;
internal bool DiffSteerFeedforwardLimitedRightFront;
internal bool DiffSteerFeedforwardLimitedRightRear;
internal long ActualThLeftFrontTimestamp;
internal long ActualThLeftFrontSequence;
internal long ActualThLeftRearTimestamp;
internal long ActualThLeftRearSequence;
internal long ActualThRightFrontTimestamp;
internal long ActualThRightFrontSequence;
internal long ActualThRightRearTimestamp;
internal long ActualThRightRearSequence;
[IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
[IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
[IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
MedullaAdapter/MCURoutine.cs
+153 -28
View File
@@ -4,7 +4,9 @@ using FundamentalLib;
using MCUSerialBridgeCLR;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Threading;
namespace MedullaAdapter
{
@@ -51,6 +53,19 @@ namespace MedullaAdapter
{
return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
}
/// <summary>
/// 保存异步CAN反馈的本机单调接收时刻和递增序号，仅供辨识日志使用。
/// </summary>
private static void MarkFeedbackReceived(
ref long timestamp,
ref long sequence)
{
Interlocked.Exchange(
ref timestamp,
Stopwatch.GetTimestamp());
Interlocked.Increment(ref sequence);
}
// M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。
public override void Operation(int iteration)
{
@@ -394,6 +409,44 @@ namespace MedullaAdapter
var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f));
var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f));
var wheelCommandSuppressed =
cart.AlarmLevel == 2 ||
cart.WaitStart ||
cart.PreparaStart ||
_driversDisabled;
var speedLimit = Math.Max(0f, cart.SendThresSpeed);
cart.SentSpeedLFL = wheelCommandSuppressed ? 0f : lfl;
cart.SentSpeedLFR = wheelCommandSuppressed ? 0f : lfr;
cart.SentSpeedLRL = wheelCommandSuppressed ? 0f : lrl;
cart.SentSpeedLRR = wheelCommandSuppressed ? 0f : lrr;
cart.SentSpeedRFL = wheelCommandSuppressed ? 0f : rfl;
cart.SentSpeedRFR = wheelCommandSuppressed ? 0f : rfr;
cart.SentSpeedRRL = wheelCommandSuppressed ? 0f : rrl;
cart.SentSpeedRRR = wheelCommandSuppressed ? 0f : rrr;
cart.WheelCommandLimitedLFL =
Math.Abs(cart.SpeedLFL) > speedLimit;
cart.WheelCommandLimitedLFR =
Math.Abs(cart.SpeedLFR) > speedLimit;
cart.WheelCommandLimitedLRL =
Math.Abs(cart.SpeedLRL) > speedLimit;
cart.WheelCommandLimitedLRR =
Math.Abs(cart.SpeedLRR) > speedLimit;
cart.WheelCommandLimitedRFL =
Math.Abs(cart.SpeedRFL) > speedLimit;
cart.WheelCommandLimitedRFR =
Math.Abs(cart.SpeedRFR) > speedLimit;
cart.WheelCommandLimitedRRL =
Math.Abs(cart.SpeedRRL) > speedLimit;
cart.WheelCommandLimitedRRR =
Math.Abs(cart.SpeedRRR) > speedLimit;
cart.WheelCommandSuppressed = wheelCommandSuppressed;
Interlocked.Exchange(
ref cart.WheelCommandTimestamp,
Stopwatch.GetTimestamp());
Interlocked.Increment(
ref cart.WheelCommandSequence);
if (iteration % 2 == 0)
{
//SendNodeGuardRequests(SendCan);
@@ -685,13 +738,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedLeftFrontLeft = speed;
cart.LFLActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x281,
"LFL",
rpm,
speed);
cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x282] = (msg) =>
{
@@ -700,13 +758,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = -ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
-BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedLeftFrontRight = speed;
cart.LFRActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x282,
"LFR",
rpm,
speed);
cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x283] = (msg) =>
{
@@ -715,13 +778,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedRightFrontLeft = speed;
cart.RFLActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x283,
"RFL",
rpm,
speed);
cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x284] = (msg) =>
{
@@ -730,13 +798,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = -ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
-BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedRightFrontRight = speed;
cart.RFRActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x284,
"RFR",
rpm,
speed);
cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x285] = (msg) =>
{
@@ -745,13 +818,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedLeftRearLeft = speed;
cart.LRLActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x285,
"LRL",
rpm,
speed);
cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x286] = (msg) =>
{
@@ -760,13 +838,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = -ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
-BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedLeftRearRight = speed;
cart.LRRActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x286,
"LRR",
rpm,
speed);
cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x287] = (msg) =>
{
@@ -775,13 +858,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedRightRearLeft = speed;
cart.RRLActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x287,
"RRL",
rpm,
speed);
cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x288] = (msg) =>
{
@@ -790,13 +878,18 @@ namespace MedullaAdapter
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
var speed = -ConvertRpm2Mps(rpm);
var positionMillimeters =
ConvertR2MM(
-BitConverter.ToInt32(payload, 0) /
10000f);
cart.ActualSpeedRightRearRight = speed;
cart.RRRActualPos = positionMillimeters;
_wheelSpeedLogger.RecordCanFeedback(
0x288,
"RRR",
rpm,
speed);
cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
speed,
positionMillimeters);
},
[0x289] = (msg) =>
{
@@ -914,10 +1007,18 @@ namespace MedullaAdapter
//Console.WriteLine("Received 0x18B CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0);
cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
: cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront;
var raw = BitConverter.ToInt32(payload, 0);
cart.ActualThLeftFront = raw >= 16384
? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
: raw / 4096f / 5f * 360 - cart.ThBiasLeftFront;
MarkFeedbackReceived(
ref cart.ActualThLeftFrontTimestamp,
ref cart.ActualThLeftFrontSequence);
_wheelSpeedLogger.RecordSteeringAngleFeedback(
0x18B,
"LF",
raw,
cart.ActualThLeftFront);
},
[0x18C] = (msg) =>
{
@@ -927,6 +1028,14 @@ namespace MedullaAdapter
cart.ActualThRightFront = raw >= 16384
? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront
: raw / 4096f / 5f * 360 - cart.ThBiasRightFront;
MarkFeedbackReceived(
ref cart.ActualThRightFrontTimestamp,
ref cart.ActualThRightFrontSequence);
_wheelSpeedLogger.RecordSteeringAngleFeedback(
0x18C,
"RF",
raw,
cart.ActualThRightFront);
//DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C");
},
[0x18D] = (msg) =>
@@ -934,20 +1043,36 @@ namespace MedullaAdapter
//Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
: cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear;
var raw = BitConverter.ToInt32(payload, 0);
cart.ActualThLeftRear = raw >= 16384
? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
: raw / 4096f / 5f * 360 - cart.ThBiasLeftRear;
MarkFeedbackReceived(
ref cart.ActualThLeftRearTimestamp,
ref cart.ActualThLeftRearSequence);
_wheelSpeedLogger.RecordSteeringAngleFeedback(
0x18D,
"LR",
raw,
cart.ActualThLeftRear);
},
[0x18E] = (msg) =>
{
//Console.WriteLine("Received 0x18E CAN Message");
var payload = msg.Payload;
if (payload == null || payload.Length < 4) return;
cart.ActualThRightRear = BitConverter.ToInt32(payload, 0);
cart.ActualThRightRear = cart.ActualThRightRear >= 16384
? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
: cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear;
var raw = BitConverter.ToInt32(payload, 0);
cart.ActualThRightRear = raw >= 16384
? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
: raw / 4096f / 5f * 360 - cart.ThBiasRightRear;
MarkFeedbackReceived(
ref cart.ActualThRightRearTimestamp,
ref cart.ActualThRightRearSequence);
_wheelSpeedLogger.RecordSteeringAngleFeedback(
0x18E,
"RR",
raw,
cart.ActualThRightRear);
},
// 远程帧
MedullaAdapter/MotorRoutine.cs
+80 -15
View File
@@ -4,6 +4,7 @@ using FundamentalLib;
using MDCSToolBox.Commons;
using System;
using System.Diagnostics;
using System.Threading;
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
namespace MedullaAdapter
@@ -75,6 +76,11 @@ namespace MedullaAdapter
cart.ClumsyControl = CartDefinition.currentPriority == 0;
// 计算四个舵轮PID和8个驱动电机最终速度。
UpdateDiffSteerWheelSpeeds();
Interlocked.Exchange(
ref cart.DiffSteerControlTimestamp,
Stopwatch.GetTimestamp());
Interlocked.Increment(
ref cart.DiffSteerControlSequence);
// 平滑更新硬件速度限制。
UpdateSendSpeedLimit();
// 更新红黄绿灯状态。
@@ -351,6 +357,15 @@ namespace MedullaAdapter
leftRear = 0f;
rightFront = 0f;
rightRear = 0f;
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
cart.DiffSteerFeedforwardLimitedLeftFront = false;
cart.DiffSteerFeedforwardLimitedLeftRear = false;
cart.DiffSteerFeedforwardLimitedRightFront = false;
cart.DiffSteerFeedforwardLimitedRightRear = false;
var currentTimestamp = Stopwatch.GetTimestamp();
@@ -365,22 +380,45 @@ namespace MedullaAdapter
deltaTimeSeconds <=
MaximumFeedforwardIntervalSeconds)
{
cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
(float)(deltaTimeSeconds * 1000.0);
leftFront = CalculateDiffSteerRateFeedforward(
cart.ThLeftFront,
_previousThLeftFront,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateLf,
out var limitedLf);
leftRear = CalculateDiffSteerRateFeedforward(
cart.ThLeftRear,
_previousThLeftRear,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateLr,
out var limitedLr);
rightFront = CalculateDiffSteerRateFeedforward(
cart.ThRightFront,
_previousThRightFront,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateRf,
out var limitedRf);
rightRear = CalculateDiffSteerRateFeedforward(
cart.ThRightRear,
_previousThRightRear,
deltaTimeSeconds);
deltaTimeSeconds,
out var targetRateRr,
out var limitedRr);
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond =
targetRateLf;
cart.DiffSteerTargetRateLeftRearDegreesPerSecond =
targetRateLr;
cart.DiffSteerTargetRateRightFrontDegreesPerSecond =
targetRateRf;
cart.DiffSteerTargetRateRightRearDegreesPerSecond =
targetRateRr;
cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
}
}
@@ -398,29 +436,44 @@ namespace MedullaAdapter
private float CalculateDiffSteerRateFeedforward(
float targetAngleDegrees,
float previousTargetAngleDegrees,
double deltaTimeSeconds)
double deltaTimeSeconds,
out float targetRateDegreesPerSecond,
out bool limited)
{
targetRateDegreesPerSecond = 0f;
limited = false;
var gain = cart.DiffSteerRateFeedforwardGain;
var wheelDistanceMillimeters =
cart.DiffSteerWheelDistanceMillimeters;
var maximumSpeed =
cart.DiffSteerRateFeedforwardMaximumSpeed;
if (!IsFinite(gain) || gain <= 0f ||
!IsFinite(wheelDistanceMillimeters) ||
wheelDistanceMillimeters <= 0f ||
!IsFinite(maximumSpeed) || maximumSpeed <= 0f ||
!IsFinite(targetAngleDegrees) ||
!IsFinite(previousTargetAngleDegrees))
if (!IsFinite(targetAngleDegrees) ||
!IsFinite(previousTargetAngleDegrees) ||
!double.IsFinite(deltaTimeSeconds) ||
deltaTimeSeconds <= 0.0)
{
return 0f;
}
// 机械舵角受限，必须使用直接差值而不是圆周最短角差。
targetRateDegreesPerSecond =
(float)((targetAngleDegrees -
previousTargetAngleDegrees) /
deltaTimeSeconds);
if (!IsFinite(gain) || gain <= 0f ||
!IsFinite(wheelDistanceMillimeters) ||
wheelDistanceMillimeters <= 0f ||
!IsFinite(maximumSpeed) || maximumSpeed <= 0f)
{
return 0f;
}
var targetRateRadiansPerSecond =
(targetAngleDegrees - previousTargetAngleDegrees) *
Math.PI / 180.0 /
deltaTimeSeconds;
targetRateDegreesPerSecond *
Math.PI / 180.0;
var wheelDistanceMeters =
wheelDistanceMillimeters / 1000.0;
var feedforwardSpeed =
@@ -429,10 +482,13 @@ namespace MedullaAdapter
targetRateRadiansPerSecond *
gain;
return (float)Math.Clamp(
var limitedSpeed = (float)Math.Clamp(
feedforwardSpeed,
-maximumSpeed,
maximumSpeed);
limited = Math.Abs(
feedforwardSpeed - limitedSpeed) > 1e-9;
return limitedSpeed;
}
/// <summary>
@@ -446,6 +502,15 @@ namespace MedullaAdapter
cart.DiffSteerRateFeedforwardLeftRear = 0f;
cart.DiffSteerRateFeedforwardRightFront = 0f;
cart.DiffSteerRateFeedforwardRightRear = 0f;
cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
cart.DiffSteerFeedforwardLimitedLeftFront = false;
cart.DiffSteerFeedforwardLimitedLeftRear = false;
cart.DiffSteerFeedforwardLimitedRightFront = false;
cart.DiffSteerFeedforwardLimitedRightRear = false;
cart.DiffSteerTotalOutputLeftFront = 0f;
cart.DiffSteerTotalOutputLeftRear = 0f;
cart.DiffSteerTotalOutputRightFront = 0f;
MedullaAdapter/WheelSpeedDiagnosticLogger.cs
+267 -8
View File
@@ -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();
MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
BIN
View File
Binary file not shown.