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

完善蟹行虚拟阿克曼与SendMotion运动坐标系，并添加轮速诊断日志

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-07-30 18:10:22 +08:00
co-authored by Cursor
parent 7e05ff098e
commit c1fe73caec
92 changed files with 1794 additions and 776 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/MCURoutine.cs
+138 -9
View File
@@ -4,6 +4,7 @@ using FundamentalLib;
using MCUSerialBridgeCLR;
using System;
using System.Collections.Generic;
using System.IO;
namespace MedullaAdapter
{
@@ -26,6 +27,9 @@ namespace MedullaAdapter
private bool io_bit4 = false;//黄灯
private const byte BatteryPortIndex = 3;
private static readonly byte[] BatteryRequest = BuildBatteryRequest();
private readonly WheelSpeedDiagnosticLogger
_wheelSpeedLogger =
new WheelSpeedDiagnosticLogger();
// M层单车底盘：将车轮线速度换算为驱动电机转速。
private static float ConvertMps2Rpm(float mps)
@@ -50,6 +54,8 @@ namespace MedullaAdapter
// M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。
public override void Operation(int iteration)
{
UpdateWheelSpeedDiagnosticState();
if (_lastIteration != iteration)
{
_lastIteration = iteration;
@@ -158,6 +164,7 @@ namespace MedullaAdapter
cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2;
cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2;
cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2;
_wheelSpeedLogger.RecordSnapshot(cart);
// M层CAN辅助：封装本周期驱动器CAN发送参数。
MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2)
{
@@ -264,8 +271,9 @@ namespace MedullaAdapter
{
_operationTime++;
return;
}
}
}
// 没有错误 没有节点保护 就发06 07 0F使能
else if (_operationTime == 4)
{
@@ -428,6 +436,79 @@ namespace MedullaAdapter
}
// M层诊断：根据界面开关创建或关闭本次轮速CSV记录。
private void UpdateWheelSpeedDiagnosticState()
{
if (cart == null)
return;
if (cart.WheelSpeedDiagnosticEnabled)
{
if (_wheelSpeedLogger.IsRunning)
return;
try
{
var configuredDirectory =
string.IsNullOrWhiteSpace(
cart.WheelSpeedDiagnosticDirectory)
? @"logs\wheel-speed"
: cart.WheelSpeedDiagnosticDirectory;
var logDirectory =
Path.IsPathRooted(configuredDirectory)
? configuredDirectory
: Path.Combine(
AppContext.BaseDirectory,
configuredDirectory);
_wheelSpeedLogger.Start(
logDirectory,
cart.CarNum);
cart.WheelSpeedDiagnosticStatus =
"记录中：" +
_wheelSpeedLogger.SnapshotLogPath;
}
catch (Exception ex)
{
cart.WheelSpeedDiagnosticEnabled =
false;
cart.WheelSpeedDiagnosticStatus =
"启动失败：" + ex.Message;
Console.WriteLine(
"轮速诊断启动失败：" +
ex.Message);
}
return;
}
if (!_wheelSpeedLogger.IsRunning)
return;
try
{
var snapshotPath =
_wheelSpeedLogger.SnapshotLogPath;
_wheelSpeedLogger.Stop();
cart.WheelSpeedDiagnosticStatus =
"已保存：" + snapshotPath;
}
catch (Exception ex)
{
cart.WheelSpeedDiagnosticStatus =
"停止失败：" + ex.Message;
Console.WriteLine(
"轮速诊断停止失败：" +
ex.Message);
}
}
// M层CAN安全：判断单个驱动节点是否处于可运行状态。
private static bool IsNodeOperational(byte remoteCode)
{
@@ -601,7 +682,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm);
var speed = ConvertRpm2Mps(rpm);
cart.ActualSpeedLeftFrontLeft = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x281,
"LFL",
rpm,
speed);
cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x282] = (msg) =>
@@ -610,7 +697,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm);
var speed = -ConvertRpm2Mps(rpm);
cart.ActualSpeedLeftFrontRight = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x282,
"LFR",
rpm,
speed);
cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x283] = (msg) =>
@@ -619,7 +712,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm);
var speed = ConvertRpm2Mps(rpm);
cart.ActualSpeedRightFrontLeft = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x283,
"RFL",
rpm,
speed);
cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x284] = (msg) =>
@@ -628,7 +727,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm);
var speed = -ConvertRpm2Mps(rpm);
cart.ActualSpeedRightFrontRight = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x284,
"RFR",
rpm,
speed);
cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x285] = (msg) =>
@@ -637,7 +742,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm);
var speed = ConvertRpm2Mps(rpm);
cart.ActualSpeedLeftRearLeft = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x285,
"LRL",
rpm,
speed);
cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x286] = (msg) =>
@@ -646,7 +757,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm);
var speed = -ConvertRpm2Mps(rpm);
cart.ActualSpeedLeftRearRight = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x286,
"LRR",
rpm,
speed);
cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x287] = (msg) =>
@@ -655,7 +772,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm);
var speed = ConvertRpm2Mps(rpm);
cart.ActualSpeedRightRearLeft = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x287,
"RRL",
rpm,
speed);
cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x288] = (msg) =>
@@ -664,7 +787,13 @@ namespace MedullaAdapter
var payload = msg.Payload;
if (payload == null || payload.Length < 8) return;
var rpm = DecodeRpmFromPayload(payload);
cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm);
var speed = -ConvertRpm2Mps(rpm);
cart.ActualSpeedRightRearRight = speed;
_wheelSpeedLogger.RecordCanFeedback(
0x288,
"RRR",
rpm,
speed);
cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
},
[0x289] = (msg) =>