First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'master' of http://git.fairylandtech.com/ruifeng.zhou/Tutorial

Browse Source
This commit is contained in:
shuai.li
2026-07-04 14:58:31 +08:00
parent 22670d916b c9342eb7f7
commit 8e3f276a24
2 changed files with 230 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/PilotDefinition.cs
+39
View File
@@ -865,6 +865,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega, rotateParams);
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
var inferredFleetCenterValid = false;
float inferredFleetCenterX = 0, inferredFleetCenterY = 0, inferredFleetCenterTh = 0;
Tuple<float, float, float> supposedPosForDiag = null;
if (isMaster)
{
@@ -877,7 +880,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
}
if (TryInferFleetCenter(out var cx, out var cy, out var cth))
{
inferredFleetCenterValid = true;
inferredFleetCenterX = cx;
inferredFleetCenterY = cy;
inferredFleetCenterTh = cth;
PublishFleetCenter(cx, cy, cth);
}
// D: 自动模式（非手动）下，若控制器给出理想车队中心，则以理想位姿作为各车 layout 目标，
// 使弧线路径上从车按各自相对曲率中心位置前馈，而非仅靠事后检测/SLAM 纠偏。
@@ -1115,6 +1124,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
var supposedPos = LessMath.Transform2D(
Tuple.Create(CenterX, CenterY, CenterTh),
Tuple.Create(layoutX, layoutY, layoutTh));
supposedPosForDiag = supposedPos;
var posBias = LessMath.SolveTransform2D(Tuple.Create(selfX, selfY, selfTh), supposedPos);
posBiasX = (float)posBias.Item1;
posBiasY = (float)posBias.Item2;
@@ -1340,6 +1350,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
var cx = xDetectCompensate + xPosCompensate;
var cy = yDetectCompensate + yPosCompensate;
var cth = thDetectCompensate + thPosCompensate;
var actualCenterThForDiag = inferredFleetCenterValid ? inferredFleetCenterTh : CenterTh;
var idealHeadingErr = (float)CommonMath.ThDiff(MultiVehicleAutoIdealTh, actualCenterThForDiag);
var idealHeadingErrReverse = (float)CommonMath.ThDiff(actualCenterThForDiag, MultiVehicleAutoIdealTh);
var frontRearDiff = (float)CommonMath.ThDiff(fleetFrontTh, fleetRearTh);
var commandHeadingSplit = frontRearDiff / 2f;
var commandBaseTh = (float)CommonMath.RoundTh(fleetRearTh + commandHeadingSplit);
var supposedPosText = supposedPosForDiag == null
? "N/A"
: $"({supposedPosForDiag.Item1:0},{supposedPosForDiag.Item2:0},{supposedPosForDiag.Item3:0.0})";
var crabDbg = isMaster && manualEnabled && fleetMode == 1
? $"| CRAB speed:{crabInputVx:F3} steerRatio:{crabInputVy:F3} raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
: "";
@@ -1367,6 +1386,21 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
DLog.Log(dbg, "MultiVehicleDbg");
FleetDiag(dbg);
if (autoMode || autoEnabled || MultiVehicleAutoEnabled)
DLog.Log(
$"APPLY car{CarNum} master:{isMaster} autoMode:{autoMode} manual:{manualEnabled} " +
$"cmd(vx:{fleetVx:0.000},f:{fleetFrontTh:0.00},r:{fleetRearTh:0.00},base:{commandBaseTh:0.00},split:{commandHeadingSplit:0.00},f-r:{frontRearDiff:0.00}) " +
$"ideal(has:{MultiVehicleAutoHasIdeal},x:{MultiVehicleAutoIdealX:0},y:{MultiVehicleAutoIdealY:0},th:{MultiVehicleAutoIdealTh:0.00}) " +
$"centerPublished({CenterX:0},{CenterY:0},{CenterTh:0.00}) " +
$"centerInferred(valid:{inferredFleetCenterValid},x:{inferredFleetCenterX:0},y:{inferredFleetCenterY:0},th:{inferredFleetCenterTh:0.00}) " +
$"idealHeadingErr(target-actual):{idealHeadingErr:0.00} reverse(actual-target):{idealHeadingErrReverse:0.00} " +
$"layout({layoutX:0},{layoutY:0},{layoutTh:0.00}) self({selfX:0},{selfY:0},{selfTh:0.00}) supposed:{supposedPosText} " +
$"detectDth:{detectDth:0.00}->comp:{thDetectCompensate:0.00} " +
$"posBiasTh:{posBiasTh:0.00}->comp:{thPosCompensate:0.00} " +
$"totalLocalComp(x:{cx:0.0},y:{cy:0.0},th:{cth:0.00}) " +
$"useDetourCorr:{useDetourCorrection} slam:{slamRead} fleetPos:{fleetPosValid} ready:{fleetReady}",
"FleetCrabHeadingDbg");
// 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
Hedingben.ToastText(
$"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
@@ -1521,6 +1555,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
return true;
}
public bool TryGetFleetCenterFromMembers(out float centerX, out float centerY, out float centerTh)
{
return TryInferFleetCenter(out centerX, out centerY, out centerTh);
}
private static (float, float, float) InferFleetCenterFromCar(VehicleSyncInfo car)
{
// 由 carWorld = Transform2D(center, layout) 反推 center = carWorld ∘ layout⁻¹。