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/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);
},
// 远程帧