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

Add fleet correction diagnostics

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-01 23:10:13 +08:00
parent 1c0258ec75
commit f127e145e1
1 changed files with 4 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/PilotDefinition.cs
+4
View File
@@ -1284,8 +1284,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
$"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " + $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
$"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} outBias({Conf.TwoLegOutputBiasX:F0},{Conf.TwoLegOutputBiasY:F0}) " + $"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} outBias({Conf.TwoLegOutputBiasX:F0},{Conf.TwoLegOutputBiasY:F0}) " +
$"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " + $"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
$"fac({Conf.MultiVehicleDetectBiasXFac:F2},{Conf.MultiVehicleDetectBiasYFac:F2},{Conf.MultiVehicleDetectBiasThFac:F2}) " +
$"lim({Conf.MultiVehicleDetectBiasXThreshold:F1},{Conf.MultiVehicleDetectBiasYThreshold:F1},{Conf.MultiVehicleDetectBiasThThreshold:F1}) " +
$"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " + $"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " +
$"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " + $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
$"fac({Conf.MultiVehiclePosBiasXFac:F2},{Conf.MultiVehiclePosBiasYFac:F2},{Conf.MultiVehiclePosBiasThFac:F2}) " +
$"lim({Conf.MultiVehiclePosBiasXThreshold:F1},{Conf.MultiVehiclePosBiasYThreshold:F1},{Conf.MultiVehiclePosBiasThThreshold:F1}) " +
$"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " + $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
$"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " + $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " +
$"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " + $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " +